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DOUNTIL: The DOUNTIL is a loop with these characteristics: 

1) The counter or other item to be "incremented” is initialized before 
entering the loop; 


2) The test is performed at the end of the loop. The conditions that 

must exist to exit from the loop are those that appear in the DOUNTIL 
test; 


3) The item to be executed must be a standard construct or a single 
statement; 

4) The counter is incremented or other increment -like action Is generally 
taken (e.g., another line is read) at the beginning of the loop. 

If "Cl” is the condition that must exist to exit from the loop and ”P1" is 
a standard construct or single statement, the DOWHILE would be written as 


PI 


DOUNTIL Cl 


DOCASE: The DOCASE construct is for executing a different set of state- 

ments for each of several different values of a variable. If "Cl" is the var- 
iable being tested and if "Cl" may have values 1, 2, or 3, the construct ap- 
pears 



Example A 


i 


T \ 


Cl=l 

/ F 


T \ 

Cl =2 

// 



■HE 

T/T 

PI 

P2 

P3 

P4 


Example B 


I 

1 Example A is equivalent to the nested IFTHENELSE form shown in B* 


Subroutine indexing. - The subroutine descriptions and VCLRs are arranged 
according to the number assigned in the block diagram. This label consists of 
three parts (nl.n2.n3). The first part (nl) indicates with which executable— 
(1) INITDRVR , (2) SIMDRVR, or (3) POSTDRVR — the module is associated. While 
some routines are used in more than one executable, each is documented only 
once and labeling number nl tells which section includes that documentation. 
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The next number (n2) indicates the level in the program hierarchy at which the 
routine occurs. There are three main levels under each executive driver and a 
fourth level that is assigned to the utility functions used by a variety of 
routines. The final number (n3) in the routine label distinguishes the modules 
within each level of one executable program. 

In-code documentation. - Although the information contained in this pro- 
grammer * s guide provides^an understanding of the overall program logic and 
function of the individual subroutines, the bulk of the program documentation 
is included in the Fortran program modules. This enhances the accessibility of 
the documentation and allows it to be updated as modifications are made. Each 
Fortran module contains a preamble that lists the routine* si 


1) 

Purpose ; 


2) 

Input (calling arguments and 

terminal inputs) ; 

3) 

Output (calling arguments and 

terminal outputs); 

4) 

Common variables; 


5) 

Internal variables; 


6) 

External references; 


7) 

Functional description; 


8) 

Assumptions and limitations; 


9) 

Special comments; 


10) 

References . 



Figure B-l illustrates an example of this in-code documentation. The file 
SKLTN.FOR contains a skeleton of the preamble for use in writing new programs. 
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ORIGINAL PAGE IS 
OF POOR QUALITY 


c**** 

PROGRAM INITDRVR 


CDO 



COO 

ROBOTICS SIMULATION (ROBSIM) PROGRAM 

CDO 

SYSTEM DEFINITION FUNCTION ROUTINE 

CDO 






CDl 



CD1 

PURPOSE 


GDI 



CDl 

The purpose of the ROBSIM Progran is to provide s 

GDI 

broed range of computer capabilities to assist in the 

CDl 

design, verification. 

simulation, and study of 

CDl 

robotic systems. 


CDl 

The progrea INITDRVR is the System Definition Function 

CDl 

driver. It operetes in 

an interactive node, prompting the 

CDl 

user for the systen definition option desired. Velid options 

CDl 

are: crests or nodify 

an am data file, crests or 

CDl 

nodify s detailed envi 

foment file, crests or modify loed 

CDl 

objects, crests s systsn dote file, or eerminece INITDRVR 

CDl 

program execution. 


CDl 






CD2 



CD2 

DEFINITION OF INPUT 


CD 2 



CD2 

CALLING ARGUMENTS 


CD2 



CD2 

SYMBOL TYPE 

DIM DEFINITION 

CD2 



CD2 

N/A 


CD2 



CD2 

TERMINAL INPUTS 


CD2 



CD2 

SYMBOL TYPE 

DIM DEFINITION 

CD2 



CD2 

IMODE 1*4 

1 Select flag for node of operation 

CD2 


■ 1, Creete/nodify an am date file 

CD2 


• 2, Create/nodify detailed 

CD2 


environment file 

CD2 


• 3, Create/modify loed objects detn 

CD2 


■ 4, Create system date file 

CD2 


• 5, End ROES IK INITDCVt executive 

CD2 

ITERM 1*4 

1 Program termination flag. 

CD2 


requested following fatal 

CD2 


error 

CD2 


* 1, Reissue program mode 

CD2 


selection prompt 

CD2 


• RETURN, Terminate program 

CD 2 



geese 



CD3 



CD 3 

DEFINITION OP OUTPUT 


CD3 



COB 

CALLING ARGUMENTS 


CD3 



CD3 

SYMBOL TYPE 

DIM DEFINITION 

CD3 



CD3 

N/A 


CD3 



CD3 

TERMINAL OUTPUTS 


CD3 



CD3 

SYMBOL TYPE 

DIM DEFINITION 

CD3 



CD3 

None 


CD3 



CDA 



CD4 

COMWN VARIABLES 


CM 



CM 

INPUT 


CM 



CM 

LUl, LU2 


CM 



CM 

OUTPUT 


CM 



CM 

NONE 


CM 






CDS 



CDS 

INTERNAL VARIABLES 


CDS 



CDS 

SYMBOL TYPE DIM 

DEFINITION 

CDS 



CDS 

IERROR 1*4 1 

Error indicator flag 

CDS 


■0, No arrors encountered 

CDS 


.NE. 0, Error has occurred. 

CDS 


contains appropriate 

CDS 


error nunber required 

CDS 


by routias ERKMSG 

CDS 

IHLP 1*4 l 

Tha intsgar 911 input from terminal Co 

CDS 


signify user wishes to access help fils 

CDS 

NODE 1*4 l 

ROBSIM progran node flag, set co l 

CDS 


co indicate program execution is 

CD5 


currently wichin the System Definition 

CDS 


Function 


CDS' 


cm external unuidu 

CS6 

CD6 I/O FILE* 

CD6 

CM LUl - Logical unit assigned for input fro* eh« terminal 

CM LU2 - Logical unit assigned for output to th« terminal 

CM 

CM SCUTCH FILES 


CM M/A 

CM 

CM EXTERNAL ROUTINES 

CM 


CM 

BLDARM 

- Create/nodify am date file 

CM 

BLDEMV 

- Creete/nodify detailed environment file 

CM 

BLDLOD 

- Creete/nodify loed objects data file 

CM 

BLDSYS 

- Create system date file 

CM 

ERIM8C 

- Prints error nasaages for any error occurring 

CM 


during execution 

CM 

LBR HELP 

- Gains access to the ROBSIM help library 

CM 

s£tlu 

- Default logical unics routine 


CM 

CD7 

CD7 FUNCTIONAL DESCRIPTION 
CD7 

CD7 Tha ROSSIM comand file prompts tHa usar for tha 

CD7 program function desired. Tha thraa ROSSIM prof ran 
CD7 functions ara Syacaa Definition, Analysis Tools, and 
CD7 Post Processing. Tha uaar nay also request prograa 
CD 7 tarnination. Upon receiving a valid function request, 

CD7 the ROSSIM comand file transfers control to and executes 
CD7 the appropriate function driver. 

CD7 The System Definition executive calls BLDARM, BLDEMV, 

CD7 BLDLOO or BLDSYS to create or nodify an arm, environment, 
CD7 load objects or ara system. 

CD 7 


C« «»* * * * »M »**i M ** M*»»«****** *««« »« »*«« *iMM » t * M« < «« a 

CDS 

CDS ASSUMPTIONS AND LIMITATIONS 
CDS 

CDS 1. ROSSIM is programmed in FORTRAN 77 for uee on 

CDS VAX 11/780 conpucers under the VMS operating 

CDS systen. 

CDS 2. ROSSIM uses Evens end Sutherland Multi Picture 

CM Systen graphics using MPS FORTRAN callable 

CM graphics routines. Use of Che graphics 

CDS capabilities in ROSSIM is optional, however 

CDS full utilisation of the prog ran eapabilicias 

CDS is greatly United wichout Che graphics. 


CDS 

CDS 



CDS 

SPECIAL COMtffNTS 


CDS 

1. If graphics is desired, 


CDS 

the graphics work station 

CDS 

must be assigned using 

individual facility 

CDS 

procedures. 

files must exist prior to 

CDS 

2. The necessary am data 

CDS 

building a system. 


CDS 




CD 10 
CD 10 
CD10 
CD 10 
CD10 


C THE F0LL0HIMG CREATES A HELP LIBEAET MODULE 


CDXl INITDBVR 

CDX THIS MODULE IS EXECUTED BY THE SYSTEM DEFINITION PROCESSOR 
CDX 

CDX2 PARAWTERS 
CDX Qualifiers: 

CDX /IMODE 
CDXJ /IMODE 
CDX TYPE 

COX 1*4 

CDX 
COX 
CDX 
CDX 
CDX 
CDX 

CDX2 FUNCTION 

CDX The ROSSIM cotmend file pronpta the user for the 

CDX progran function desired* The three ROBSIM progran 
CDX functions are Systen Definition, Analysis Tools, and 
CDX Poat Processing. The user nay also requast progran 
CDX tarnination. Upon receiving s valid function request, 

CDX the ROBSIM coensnd file tranefers control to and axacutss 
CDX the appropriate function driver. 

CDX The Systen Definition executive calls BLDaRM, 3LDENV , 

CDX BLDLOO or BLDSYS to craate or nodify an ana, anviroruasne , 
CDX load objects or arm system. 

CDX 


DIM DEFINITION 

l Select flag for nods of operation 

* 1, Create/nodify an in data file 

■ 2, Creete/nodify detailed 

environment file 

• 3, Create/nodify load objects data fils 

■ 4, Crests systsn data file 

- 5, Ead ROBSIM INITDRVR executive 


Figure B-l.- 


Example of 


in-code documentation. 
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Implementation Notes 


This section describes the programming conventions used in implementing 
ROBSIM on the VAX-11 computer architecture under the VMS operating system. 

The program consists of a large number of FORTRAN routines and their compiled 
object modules, along with a limited number of executable images and VMS com- 
mand files. 

Executive-level co™n*od file . - The executive level of ROBSIM is handled 
by an interactive command file ROBSIM. COM. Figure B-2* shows this command 
file. This file runs one of the ROBSIM executable i m ages (Fig. B-3) selected 
by the user. PREPDRVR.EXE is designed as a driver for preprocessing data that 
may be used by the other program executables. INITDRVR.EXE is the executable 
containing the system definition routines and SIMDRVR.EXE contains the analy- 
sis tools image. The postprocessor functions reside in two executable files: 
(1) POSTDRVT.EXE for video-terminal display of results, and (2) POSTDRHP.EXE 
for hardcopy plotting. This is because the display software requires linking 
of different modules for terminal vs hardcopy displays. 



Figure B-3.- ROBSIM executive command file. 


Linking the programs . - Each executable image contains an executive rou- 
tine having the same name as the executable (POSTDRVR for the postprocessor 
images) and a large number of supporting routines. To facilitate linking, the 
compiled object modules are included in object libraries. The programs are 
linked by executing command files that reference these libraries. Figure B-4 
shows the linker command files and Table B-I lists the programs in the object 
libraries. 

The bulk of the libraries are contained in the main ROBSIM directory 
(ROBSIM_DIR: ) but some reside in different directories or devices. This is 
especially convenient for implementations on multiple-disk systems. Table 
B-II lists the alternate device specifications; these logicals must be assign- 
ed to appropriate physical devices, possibly during the log-in procedure. 


* 



Full-page figures can be found at the end of this section. 
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TABLE B-I. NODULES IN ROBSIN OBJECT LIBRARIES 


CNTLLIB.OLB OBJECT LIBRARY MODULES: 

CMPCTRL 

CNTRSIG 

CONTROL 

FORREF 

FORTOR 

ICVTATD 

PIDCON 

POSSENS 

PIDFOR 

PIDINIT 

CRLIB.OLB OBJECT LIBRARY MODULES: 

BLDARM 

BLDDAT 

BLDENV 

BLDLOD 

BLDTRG 

BLDSYS 

CREATARM 

DEFSPJT 

ENVIR 

LOAD 

RDARM 

RDARMS 

RDENV 

RDENVS 

RDLOAD 

RDLODS 

RDTARG 

RDTRGS 

TARGET 

TOTMAS 

WRTARM 

WRTENV 

WRTTRG 

WRTLOD 

WRTSYS 

EANDSLIB.OLB OBJECT LIBRARY MODULES: 

DATOUT 

DIALS 

ESCNTRL 

ESPAUS 

FORM 

GRAFIX 

ISBIT1 

ROTMAT 

LOGO 

POSTGRAF 

GEOMLIB.OLB 

OBJECT LIBRARY 

MODULES: 

ACTUATOR 

BASE 

BASELK 

BASES 

JOINT 

LINK 

LOCMOD 

LOCTMOD 

OBJECT 

SPAN 

TOOLLK 

TLDMAS 

TOOLJT 

GRAFLIB.OLB 

OBJECT LIBRARY 

MODULES: 

CADOBJ 

CYL 

DATATAB 

DBAS 

DRAW 

DRWENV 

DRWLOD 

DRWTRG 

ESMAT 

FILLET 

GRAPH 

GRTERM 

MAT 

MATVEC 

OBSTCL 

ORIENT 

RCTSTR 

RECT 

SYSGRAF 

TARG 

TRISTR 

MATHLIB . OLB 

OBJECT LIBRARY 

MODULES: 

CETM 

CRPD 

DOT2 

GAUSS 

MATMPY 

REPCOL 

SLVLIN 

SLVLIN2 

SOLVE 
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TABLE B-I. (cont) 


ESCADLIB.OLB OBJECT LIBRARY MODULES: 

GRAFCAD 



POSTLIB.OLB 

OBJECT LIBRARY MODULES: 

AXES 

HDWMOTIN 

LDTHET 

MINMAX 

ROBPLT 

SCAL 

SIMOTION 

TICMRK 


REQLIB.OLB OBJECT LIBRARY MODULES: 


ACTORQ 

BASGMNT 

BASINIT 

BCNTRLR 

BORERR 

BPCNTRL 

BRCNTRL 

CABSM 

CHKLMT 

CNSTFOR 

CVTIN 

DEBUG 

DYNAM 

ENDREQ 

FORCE 

FTIN 

JTPOS 

LODMOV 

ORERR 

OUTREQ 

PCNTRL 

POSGRDJT 

POSSPJT 

PTACC 

PVASPJT 

RATEPRO 

RCNTRL 

REQOPT 

REQPLT 

REQPRT 

REQSOF 

REQTRQ 

SPRGFOR 

SPR6INC 

TORQUE 

VAGRDJT 

SETLIB.OLB 

OBJECT LIBRARY MODULES 

• 

• 

ADDMAS 

ADDMAS2 

BASPUT 

GRASP 

RCICR 

RDSYS 

RELEAS 

SETUP 

SETUP2 

SIMLIB.OLB 

OBJECT LIBRARY MODULES 

• 

• 

ACTIVPIH 

BDRTORQ 

CALCI 

DEFCNST 

DERIV 

DOT 

DRTORQ 

EFINRT 

EFINRT2 

ENDSIM 

INITCO 

INTGRT 

LDVOLT 

NLINK 

OUTSIM 

PRTARM 

RESPON 

SETCNST 

SIMLMT 

SIMOPT 

SIMPLT 

SIMPRT 

SLVMBAS 

SLVTHDD 

SLV2ARM 

STOPFR 


UTILLIB.OLB OBJECT LIBRARY MODULES: 

CVTUNIT 

DEFUNIT 

ERRMSG 

JACOB 

LININT 

OUTUN 

SETLU 

ZERCOM 
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TABLE B-I. (concl) 


PREPLIB.OLB OBJECT LIBRARY MODULES: 
BLDCAD 

CADLIB.OLB OBJECT LIBRARY MODULES: 

CURVE LINE POINT 

TRANS F SPLINE 


TASKLIB.OLB OBJECT LIBRARY MODULES 
ANGLES CNTRLR 

INITTAR NEWFRAME 

REQUIR SEGINIT 

SGMNT TASK 


HARALICKR 

PERSPECT 

SEGTASK 

TRACKING 
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TABLE B— II* - DEVICE -DIRECTORY SPECIFICATIONS IN ROBSIM 


Logical 

Modules contained in directory 

ROBSIM DIR: 

Basic ROBSIM modules 

MPS DIR: 

Evans and Sutherland graphics modules 

HELP DIR: 

Help utility modules 

DI3000JDIR: 

Display modules (video-terminal and 


hardcopy) 

HARDC 0PY_D IR : 

Modules for creating meta-files from 


picture files 


Fortran files and object files , - Each Fortran routine is included in its 
own file; the name of the file is that of the routine it contains and the file 
type is ".FOR". An object file with the same name and type ".OBJ" holds the 
compiled version of each Fortran file. After a Fortran module is modified, an 
executable image containing the updated version can be obtained by issuing the 
following commands: 

FORTRAN MODULE NAME 

LIBRARY/REPLACE LIBNAME MODULENAME 

0LNKNAME 

Command files FORROB.COM and REPLIB.COM exist for compiling the entire 
set of routines in the main directory and updating the object libraries. 
PRTROB.COM provides for printing all of the Fortran modules. Each of these com- 
mand files should be updated when routines are added to or deleted from the 
program. 

Fortran COMMON blocks . - The variables used by several routines are ar- 
ranged into COMMON blocks. The text file ROBCOM.DOC lists and briefly de- 
scribes the variables included in each COMMON block. A Fortran COMMON state- 
ment for each block resides in a file of type ".CMN" that has the same name as 
the COMMON block. The COMMON blocks are included during compilation of the 
Fortran modules using the INCLUDE statement. This allows a block to be modified 
by changing only the ,, *CMN” file, instead of all the Fortran modules that use 
this block. 

During compilation of the Fortran modules, maximum values must be speci- 
fied for some of the array dimensions. These maximum dimensions are often de- 
fined by PARAMETER statements, and most of these statements are included in 
the file MXPRMS.TXT. Figure B-5 shows a listing of this file. To change the 
maximum dimension of some variables (e.g., to increase the number of arms pos- 
sible in a system), the programmer must only change the appropriate parameter 
(MX ARMS) and recompile the programs. 
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MXPRMS.TXT 

THE PARAMETERS IN THIS INCLUDE FILE ARE CONSTANT VALUES REQUIRED 
BY THE PROGRAM. 

INTEGERS MAXSPN, IFACTCAD, ISEGCAD 

PARAMETER (MXLNKS=10,MXARMS=5) 

PARAMETER (MAXORD=3 ,MAXSEG=20 ,MXPLTS=31 ) 

PARAMETER (MXENCMPS=30 ,MXGRCMP=40 ,MXLDCMPS=10 ) 

PARAMETER <MXPTS=10,MXL0DS=10) 

PARAMETER (MXTRGCMPS=10,MXTRGS=10) 

PARAMETER (MXGRAFPT=5000) 

PARAMETER (MAXTRAN=50 ,MAXLIN=20000 ,MAXPNT=40) 

PARAMETER (MAXARC=300,MAXSPL=200) 

PARAMETER (IFLGEND=5557) 

PARAMETER (IDIV=10,ISEGCAD=1,IFACTCAD=10) 

PARAMETER ( ITLNGTH=4 ,MAXFLD=30000 ,MAXSPN=500 ,MAXBRK=4) 

DEFINITIONS 


SYMBOL 

TYPE 

DIMENSION 

DEFINITION 

MXARMS 

1*4 

1 

MAXIMUM NUMBER OF ARMS 

MXLNKS 

1*4 

1 

MAXIMUM NUMBER LINKS ALLOWABLE 

MXPLTS 

1*4 

1 

MAXIMUM NUMBER OF Y ARRAY DATA 
PARAMETERS WHICH MAY BE 
WRITTEN TO PLOT FILE 

MAXORD 

1*4 

1 

ORDER OF THE POLYNOMIAL DESCRIBING 
THE MOTION TIME HISTORY 

MAXSEG 

1*4 

1 

MAXIMUM NUMBER OF TIME SEGMENTS 
ALLOWED TO DESCRIBE THE MOTION 
TIME HISTORY 

MXPTS 

1*4 

1 

MAXIMUM NUMBER OF POINT MASSES 
IN EACH LINK OR LOAD 

MXGRCMP 

1*4 

1 

MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
ALLOWED PER LINK 

MXENCMPS 

1*4 

1 

MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
IN ENVIRONMENT 

MXLDCMPS 

1*4 

1 

MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
IN EACH LOAD OBJECT 

MXLODS 

1*4 

1 

MAXIMUM NUMBER OF LOAD OBJECTS ALLOWED 

MXTTRCMPS 

1*4 

1 

MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
IN EACH TARGET OBJECT 

MXTRGS 

1*4 

1 

MAXIMUM NUMBER OF TARGET OBJECTS 
ALLOWED 

MXGRAFPT 

1*4 

1 

MAXIMUM NUMBER OF GRAFIX POINTS ALLOWED 
IN EACH COMPONENT 

ISEGCAD 

1*2 

1 

NUMBER OF SEGMENTS FOR THE CAD /CAM 
GRAPHICS 

IFACTCAD 

1*2 

1 

SCALE FACTOR FOR THE CAD/CAM GRAPHICS 

MAXSPN 

1*2 

1 

MAXIMUM SPAN OF THE ARM DURING CAD /CAM 


GRAPHICS 
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ITLNGTH 

1*4 

i 

LENGTH OF THE CAD/CAM FILE TITLE 

IFLGEND 

1*4 

i 

CAD/CAM FILE FLAG SPECIFYING END OF 
DIRECTORY DATA SECTION IN FILE 

MAXFLD 

1*4 

i 

MAXIMUM NUMBER OF LINES IN THE CAD /CAM 
DATA FILE 

MAXARC 

1*4 

1 

MAXIMUM NUMBER ALLOWABLE CAD/CAM 
ARC ENTITIES 

IDIV 

1*4 

1 

NUMBER OF STRAIGHT LINE SEGMENTS 
INTO WHICH CURVE ENTITY IS DIVIDED 
FOR CAD/CAM GRAPHICS DISPLAY 

MAXLIN 

1*4 

1 

MAXIMUM NUMBER ALLOWABLE CAD/ CAM 
LINE ENTITIES 

MAXPNT 

1*4 

1 

MAXIMUM NUMBER ALLOWABLE CAD/CAM 
POINT ENTITIES 

MAXSPL 

1*4 

1 

MAXIMUM NUMBER ALLOWABLE CAD/CAM 
B-SPLINE ENTITIES 

MAXBRK 

1*4 

1 

MAXIMUM ORDER ALLOWED FOR B-SPLINE 
DEFINING EQUATION DURING CAD/CAM 

MAXTRAN 

1*4 

1 

MAXIMUM NUMBER ALLOWABLE CAD/CAM 
TRANSFORMATION ENTITIES 


Figure B-5. - Listing of MXPRMS.TXT. 


Interactive help utility . - An interactive help utility is implemented in 
ROBSIM to provide online assistance to the user for answering some of the pro- 
gram prompts. The utility provides the user with information on the function 
and form of the routine and its arguments. The utility is implemented using a 
mixture of custom software and the VMS help utility. Information for the help 
library is included in the in-code documentation in the FORTRAN modules under 
the heading "CDX". The command file MAINHLP.COM is executed to set up a help 
library ROBLIB.HLB from this documentation. The executable image MNEXTRACT.EXE 
is run to extract the help library information from the FORTRAN modules; it 
selects all program lines beginning with "CDX" and deletes the "CDX" headings. 
The formatted file MAINHLP.DOC lists the FORTRAN modules (type ".FOR" implied) 
to be searched for help library information. All segments that are extracted 
must follow the conventions for creating help libraries as described in VAX/VMS 
Volume 4A Program Development Tools Utilities Reference Manual . Section 10.3.2. 
They are temporarily stored in a file of type ".HLP". 
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(Warning - all ,, .HLP M routines are deleted by the command file execution!) 

As an example. Figure B-6 shows the help documentation extracted from pro- 
gram POSTDRVR. The help utility is accessed within the ROBSIM program modules 
by a call to the subroutine LBR_HLP. This module is included in the object 
library HELP_DIR:QESTLIB.OLB along with the other routines needed for the help 
utility. Table B-III summarizes the main files employed by the help utility. 


1 POSTDRVR 

THIS MODULE IS EXECUTED BY THE POST PROCESSOR 

2 PARAMETERS 

Qualifiers: 

/IMODE 

3 /IMODE 

TYPE DIM DEFINITION 

1*4 1 Select flag for mode of operation 

* 1, Replay simulation graphic motion 

only 

* 2, Replay simulation versus hardware 

motion 

“ 3, Parameter versus parameter plots 

* 4, Return to ROBSIM executive 

2 FUNCTION 

The result of executing option 1, is to call subroutine 
SIMOTION which provides a replay of the robotic system motion 
produced from a simulation run. Option 2 provides a 
comparison of motion resultant from direct hardware theta value 
read and motion resultant from simulation execution, through 
subroutine HDWMOTIN. 

If option 3 is selected, subroutine ROBPLT is called to 
provide parameter versus parameter plots of any of the 
data computed and written to a plot file during the 
Requirements Analysis Tools Function. 

Option 4 returns execution to the primary ROBSIM level. 


Figure B-6. - Listing of help documentation extracted from POSTDRVR. 


TABLE B-III. - FILES USED FOR INTERACTIVE HELP UTILITY 


MAINHLP.COM 

MAINHLP.DOC 
MODULENAME . HLP 

ROBLIB.H LB 
LIBJiELP.FOR 
HELP MAC .MAR 


Executive command file for extracting help 

library documentation from FORTRAN code 

Names of FORTRAN modules containing help information 

Temporary file of help documentation extracted 

from routine MODULENAME. FOR 

Data file used for help utility 

Module containing program which reads ROBLIB.HLB 
Macro routine used to access help facility 





Rev A, October 1985 


Hardcopy utility . - The ROBSIM program provides the capability for inter- 
active display of the manipulator system on an Evans and Sutherland graphics 
workstation during system creation or analysis. In addition, plots of this 
display can be generated on a hardcopy plotter for future reference. Genera- 
tion of the hardcopy plot entails three steps: (1) creation during program 
execution of a picture file, (2) conversion of this file into a graphics 
meta-file, and (3) translation of this meta-file into a display or plot. 

The first step is initiated by a call to the routine HARD_COP at the 
points in the program where a hardcopy of the E&S display may be desired. If 
the user selects to keep a hardcopy of the current display, a picture file 
named by the user is created. The routines for this procedure are in object 
libraries HCPIC.OLB and HCMFL.OLB in directory HARDCOPY_DIR : and are linked 
with the ROBSIM executable images (Fig. B-4). Program execution continues 
after the meta-file is completed. 

After the ROBSIM run terminates, the user can activate programs that con- 
vert the picture files into meta-files by executing HCMFL.EXE. The resulting 
picture file can be translated into a display on the video-terminal or hard- 
copy plotter using DI3000 software. The images TRANSLATE.EXE and then 
VTMETTRNS.EXE or HPMETTRNS.EXE in device-directory DI3000_DIR: perform this 
translation. Activating these images is made easier by assignments in the 
log-in command file: 

HCMFL := RUN HARDCOPY DIR: HCMFL.EXE 

TRANSLATE := RUN DI3000DIR: TRANSLATE . EXE 

HPMETTRNS := RUN D 1 3000_DIR: HPMETTRNS.EXE 

VTMETTRNS := RUN D 1 3000_D IR : VTMETTRNS . EXE 

The user need only type the keyword to start execution of these programs. 

File "type" conventions . - The different types of files used in creating 
the ROBSIM program are designated by individual file-type suffixes in their 
file specifications. It is recommended that the programmer and user maintain 
these conventions in the files they create. Table B-IV lists the suggested 
type specifications. 
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TABLE B-IV. - FILENAME CONVENTIONS USED IN ROBSIM 


APPENDAGE 
FOR FILENAME 

DEFINITION 

.ARM 

ARM GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.SYS 

SYSTEM GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.LOD 

LOAD GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.ENV 

ENVIRONMENT GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.OBS 

OBSTACLE FILE (NONPLANARX, Y,Z COORDINATES) 
READ FOR DETAILED GEOMETRY INPUT 

.ACT 

ACTUATOR DEFINITION INPUT FILE 
READ BY MODULE ACTUATOR DURING 
SYSTEM DEFINITION 

.CON 

RESPONSE ANALYSIS CONTROL OPTIONS 
INPUT FILE READ BY CONTRL MODULE 

.THT 

HARDWARE THETA ANGLE INPUT FILE 
CREATED FROM HARDWARE CONVERSION 
ROUTINE, AND INPUT DURING POST PROCESSING 

.VLT 

HARDWARE VOLTAGE CONTROL SIGNAL INPUT 
FILE CREATED FROM HARDWARE CONVERSION 
ROUTINE, AND INPUT DURING RESPONSE 
ANALYSIS EXECUTION 

• THP 

TIME HISTORY PROFILE FILE CREATED BY 
AN INPUT TO REQUIREMENTS ANALYSIS 

.DAT 

REQUIRMENTS OR RESPONSE SIMULATION 
OPTIONS INPUT FILES; ALSO SOME OUTPUT FILES 

.CMN 

COMMON BLOCKS INCLUDED THROUGHOUT PROGRAM 

.OLB 

PROGRAM LIBRARY FILES 

• LIS 

LISTINGS OF SUBROUTINES IN EACH LIBRARY 

.FOR 

FORTRAN CODE 
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FORTRAN OBJECT MODULES 

ROBSIM PROGRAM AND UTILITIES EXECUTABLES 

COMMAND FILES FOR COMPILING, REPLACING 
MODULES IN APPROPRIATE LIBRARIES, 

LINKING THE DRIVERS, PRINTING ALL 
MODULES, AND RUNNING THE PROGRAMS 

PARAMETER FILES INCLUDED IN MODULES 
THROUGHOUT PROGRAM 

DOCUMENTATION FILES 

USER HELP FILES GENERATED WITH THE 
MNEXTRACT UTILITY IN HELPER DIRECTORY 
ACCESSIBLE WITH THE LBR HELP UTILITY 

SIMULATION PRINT OUTPUT FILES 

PLOT OUTPUT FILES FOR HEWLETT PACKARD 
X-Y PLOTTER OR VT125 GRAPHICS TERMINALS 

ARM GEOMETRY PRINT OUTPUT FILES CREATED 
DURING SYSTEM DEFINITION 

PICTURE FILES OF EVANS AND SUTHERLAND 
DISPLAYS, GENERATED WITH HARD COP 
ROUTINE; MAY BE REPRODUCED ON THE 
HEWLETT PACKARD PLOTTER AFTER 
CONVERSION TO META-FILE FORMAT 

SIMULATION OUTPUT FILE FOR POST PROCESSING 

ACCELERATION-VELOCITY-THETA OUTPUT FILE 

TORQUE OUTPUT FILE 

ACTUAL HARDWARE OUTPUT FILES FOR 
VOLTAGE CONTROL SIGNALS AND CORRESPONDING 
THETA ANGLE VALUES 

BASE MOTION PROFILE CREATED BY 
AN INPUT TO REQUIREMENTS ANALYSIS 

BASE REACTION TORQUES AND FORCES OUTPUT FILE 

PRINTOUT FILE OF DETAILED GEOMETRY DATA 
INPUT BY USER 
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TABLE B-lV (concl) 


.CAD 

DETAILED GRAPHICS COMPONENT DATA GENERATED 


FROM CAD/CAM FILE INTERFACE IN PREPROCESSOR 

.TRG 

TARGET GEOMETRY FILE CREATED DURING 


SYSTEM DEFINITION 

.SGF 

SYSTEM GEOMETRY PRINT OUTPUT FILES CREATED 


WITH UTILITY RDWRTSYS 
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IR0BSIM.COM 
$SET NOVERIFY 
$SET TERM/NOBROAD 
$C0UNT=0 
$L00PS : 

$COUNT=COUNT+ 1 

$IF COUNT. GT.l THEN GOTO ASKNEXT 
$ASKWICH: 

$WRITE SYS$OUTPUT "INPUT (PREPDRVR) — TO RUN ROBSIM PREPROCESSOR 

FUNCTION" 

$ WRITE SYS$OUTPUT "INPUT ( INITDRVR) — TO RUN ROBSIM SYSTEM 

DEFINITION FUNCTION" 

$WRITE SYS$OUTPUT " (SIMDRVR) — TO RUN ROBSIM 

SIMULATION ANALYSIS 
TOOLS FUNCTION" 

$WRITE SYS$OUTPUT " (POSTDRVR)— TO RUN ROBSIM POST 

PROCESSOR FUNCTION" 

$PROMPT : =WH I CH : 

$READ/PROMPT="" PROMPT'" SYS$COMMAND WHICH 
$IF WHICH. EQS. "INITDRVR" THEN GOTO INIT 
$IF WHICH. EQS. "SIMDRVR" THEN GOTO SIM 
$IF WHICH. EQS. "POSTDRVR" THEN GOTO POST 
$PREP 

$ASSIGN TT SYS$ INPUT 
$ASSIGN TT SYS$OUTPUT 
$RUN [ROBSIM. WORK] PREPDRVR 
$DEASSIGN SYS$ INPUT 
$DEASSIGN SYS$OUTPUT 
$INQUIRR 

$PROMPT : = INPUT ( Y ) TO RUN PREPROCESSOR FUNCTION AGAIN, - 
(OTHERWISE, RETURN) 

$READ/PROMPT="’ 'PROMPT'" SYS$COMMAND WHICH 
$IF WHICH. EQS. "Y" THEN GOTO PREP 
$GOTO LOOPS 
$INIT: 

$ASSIGN TT SYS$ INPUT 
$ASSIGN TT SYS$OUTPUT 
$RUN [ROBSIM. WORK] INITDRVR 
$DEASSIGN SYS$ INPUT 
$DEASSIGN SYS$OUTPUT 
$INQUIRI : 

$PROMPT:= INPUT (Y) TO RUN SYSTEM DEFINITION FUNCTION AGAIN, - 
(OTHERWISE, RETURN) 

$READ/PROMPT="" PROMPT'" SYS$COMMAND WHICH 
$IF WHICH. EQS. "Y" THEN GOTO INIT 
$GOTO LOOPS 
$SIM: 

$ASSIGN TT SYS$ INPUT 
$ASSIGN TT SYS$OUTPUT 


Figure B-2. - The ROBSIM executive command file. 
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$RUN [ ROBS IM. WORK ]SIMDRVR 
$DEASSIGN SYS$ INPUT 
$DEASSIGN SYS$OUTPUT 
$INQUIRS: 

$PR0MPT:= INPUT (Y) TO RUN SIMULATION ANALYSIS TOOLS - 
FUNCTION AGAIN, (OTHERWISE, RETURN) 

$READ/PROMPT=" " PROMPT'" SYS$COMMAND WHICH 
$IF WHICH. EQS."Y" THEN GOTO SIM 
$GOTO LOOPS 
$POST: 

$ WRITE SYS$OUTPUT "DO YOU WISH (1) TERMINAL OR 

(2) HARDCOPY PLOTTING?" 

$ PROMPT : = ENTER INTEGER: 

$READ/PROMPT="" PROMPT'" SYS$COMMAND WHICH 
$ASSIGN TT SYS$INPUT 
$ ASSIGN TT SYS$OUTPUT 

$IF WHICH. EQS."1" THEN RUN [ ROBS IM. WORK] POSTDRVT 
$IF WHICH.EQS."2" THEN RUN [ ROBS IM. WORK ]POSTDRHP 
$DEASSIGN SYS$ INPUT 
$DEASSIGN SYS$OUTPUT 
$INQUIRP: 

$PR0MPT:= INPUT (Y) TO RUN POST PROCESSOR FUNCTION AGAIN, - 
(OTHERWISE, RETURN) 

$READ/PROMPT=" " PROMPT"' SYS$COMMAND WHICH 
$IF WHICH. EQS."Y" THEN GOTO POST 
$GOTO LOOPS 
$ASKNEXT: 

$PR0MPT:= INPUT (Q) IF YOU WISH TO EXIT THE PROGRAM 

(OTHERWISE, RETURN) 

$READ/PROMPT="' 'PROMPT'" SYS$COMMAND QUIT 
$IF QUIT. EQS. "" THEN GOTO ASKWICH 
$EXIT 
$STOP 


Figure B-2. (concl) 
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LNKPREP.COM 

$LINK/EXECUTABLE=PREPDRVR PREPDRVR,- 
PREPLIB/LIB,- 
CADLIB/LIB,- 
ESCADLIB/LIB,- 
UTILLIB/LIB,- 
MATHLIB/LIB,- 
HCPIC/LIB,- 
HCMFL/LIB,- 

DISK$USER1 : [ ROBS IM . HELPER ] QESTL I B / L I B , - 
SYS$SYSDEVICE: [MPSGSP]MPLIB/L 

LNKINIT.COM 

$LINK/EXECUTABLE=INITDRVR INITDRVR,- 
CRLIB/LIB,- 
GEOMLIB/LIB,- 
GRAFLIB/LIB,- 
SETLIB/LIB,- 
EANDSLIB/LIB,- 
UTILLIB/LIB,- 
MATHLIB/LIB,- 
HCPIC/LIB,- 
HCMFL/LIB,- 

D I SK$USER 1 : [ ROBS IM . HE LPER ] QESTL I B / L I B , - 
SYS$SYSDEVICE: [MPSGSP]MPLIB/L 

LNKSIM.COM 

$L INK/ EXECUTABLE=S IMDRVR SIMDRVR,- 

SIMLIB/LIB,- 
CNTLLIB/LIB,- 
TASKLIB/LIB,- 
REQLIB/LIB,- 
EANDSLIB/LIB,- 
UTILLIB/LIB,- 
SETLIB/LIB,- 
MATHLIB/LIB,- 
HCPIC/LIB,- 
HCMFL/LIB,- 

DISK$USER1 : [ ROBS IM . HELPER ] QESTLIB/ LIB , - 
SYS$SYSDEVICE: [MPSGSP]MPLIB/L 


Figure B-4 - ROBSIM linker command files. 


B-16 



Rev A, October 1985 


LNKPOSTVT.COM 

$LINK/EXECUTABLE=POSTDRVT POSTDRVR,- 
POSTLIB/LIB,- 
SETLIB/LIB,- 
UTILLIB/LIB,- 
MATHLIB/LIB,- 
EANDSLIB/LIB,- 
HCPIC/LIB,- 
HCMFL/LIB,- 

DISK$USER1 : [ ROBS IM.HELPER]QESTLIB/ LIB, - 
SYS$SYSDEVICE: [MPSGSP]MPLIB/L 
di3_link:DILIB/LIB,MFN0DE/0PT, LVLC/OPT,- 
DI 3DDR : DDR1 25 , - 
DI3 LINK :UTILLIB/ LIB 


LNKPOSTHP.COM 

$LINK/EXECUTABLE=POSTDRHP POSTDRVR,- 
POSTLIB/LIB,- 
SETLIB/LIB,- 
UTILLIB/LIB,- 
MATHLIB/LIB,- 
EANDSLIB/LIB,- 
HCPIC/LIB,- 
HCMFL/LIB,- 

DISK$USER1 : [ROBSIM . HELPER]QESTLIB/ LIB 
SYS$SYSDEVICE: [MPSGSP]MPLIB/L 
D I 3_CS : MGRAIN , Q3 ATOC , - 

di3_link:DILIB/LIB,MFN0DE/0PT,LVLC/0PT,- 

DI3_DDR:DDR721 

DI3 LINK:UTILLIB/LIB 


Figure B-4. (Concl) 
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The program INITDRVR is the system 
shows the program modules employed in I 
modules. The set of functional descrip 
following pages describe these routines 



ZERCOM 


DEFUNIT 


- ERRMSG 



LOGO fl.4.6 


— E&S Routines 




- MATMPY 



- ERRMSG 



HARD_ COP 




— E&S Routines 



ERRMSG 


ERRMSG - ERR* 


DEFUNIT 


CVTUNIT 



RDTRGS 


LOCTMOD 


- ERRMSG 


- MATMPY - ERRMSG 


CVTUNIT CVTUNIT 


ROTMAP 


MATMPY 



FGLDCUT FRAME 


FRAME 


















Figure B-7. (cont) 
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Figure B-7. (concl) 
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TABLE B-V. - PROGRAMS EMPLOYED IN INITDRVR 


1.0 

INITDRVR 

1.3.1 

GRINIT 

1.4.1 

CVTUNIT 



1.3.2 

SPAN 

1.4.2 

MATMPY 

1.1.1 

SETLU 

1.3.3 

BASE 

1.4.3 

ERRMSG 

1.1.2 

LBR HELP 

1.3.4 

OBJECT 

1.4.4 

ROTMAT 

1.1.3 

BLDARM 

1.3.5 

GRAPH 

1.4.5 

CETM 

1.1.4 

BLDENV 

1.3.6 

DIALS 

1.4.6 

LOGO 

1.1.5 

BLDLOD 

1.3.7 

JOINT 

1.4.7 

CRPD 

1.1.6 

BLDSYS 

1.3.8 

ACTUATOR 



1.1.7 

BLDTRG 

1.3.9 

LINK 





1.3.10 

DEFSPJT 



1.2.1 

ZERCOM 

1.3.11 

TOOLJT 



1.2.2 

DEFUNIT 

1.3.12 

TOOLLK 



1.2.3 

CREATARM 

1.3.13 

TOTMAS 



1.2.4 

RDARM 

1.3.14 

ADDMAS 



1.2.5 

BLDDAT 

1.3.15 

ADDMAS2 



1.2.6 

WRTARM 

1.3.16 

RCICR 



1.2.7 

ENVIR 

1.3.17 

GRTERM 



1.2.8 

RDENVS 

1.3.18 

BASPUT 



1.2.9 

RDARMS 

1.3.19 

JACOB 



1.2.10 

BASES 

1.3.20 

DATOUT 



1.2.11 

SETUP 

1.3.21 

FORM 



1.2.12 

SETUP2 

1.3.22 

CYL 



1.2.13 

HARD GOP 

1.3.23 

RECT 



1.2.14 

SYSGRAF 

1.3.24 

TRISTR 



1.2.15 

RDLODS 

1.3.25 

DATATAB 



1.2.16 

LOGMOD 

1.3.26 

FILLET 



1.2.17 

WRTSYS 

1.3.27 

OBSTCL 



1.2.18 

RDENV 

1.3.28 

ORIENT 



1.2.19 

DRWENV 

1.3.29 

MAT 



1.2.20 

WRTENV 

1.3.30 

MATVEC 



1.2.21 

RDLOAD 

1.3.31 

DRAW 



1.2.22 

LOAD 

1.3.32 

ESMAT 



1.2.23 

TLDMAS 

1.3.33 

DBAS 



1.2.24 

DRWLOD 

1.3.34 

BASELK 



1.2.25 

WRTLOD 

1.3.35 

RCTSTR 



1.2.26 

RDTRGS 

1.3.36 

CADOBJ 



1.2.27 

LOCTMOD 

1.3.37 

TARG 



1.2.28 

RDTARG 





1.2.29 

TARGET 





1.2.30 

DRWTRG 





1.2.31 

WRTTRG 
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1.0 INITDRVR 

The program INITDRVR is the system definition function driver. It ope- 
rates in an interactive mode, prompting the user for the system defini- 
tion option desired — create or modify an arm data file, create or 
modify a detailed environment file, create or modify a target objects 
file, create or modify a load objects file, create a system data file, 
or terminate INITDRVR execution. Subroutine SETLU is called to set the 
Fortran logical units. The necessary simple cylinder or detailed sin- 
gle arm file must exist prior to building a system. A detailed graph- 
ics save file is opened if requested. 
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1.1.1 SETLU 


SETLU is called from Che various executive drivers Co sec Che Fortran 
logical unit number Co be scored in COMMON block LUNITBK for reference 
by Che rest of Che ROBSIM program. After assigning Che variables Co 
consecutive unit numbers, Che "unit open" flags are reset Co indicate 
Che units are not open (except Che terminal read and write unics). 



RSSIGN LU1 THRU LU20 SUCCESSIVE 
LOGICAL NUMBERS STRRTING WITH S 

CLU1-5. ... 3 


SET FLAGS FOR LU1 AND LU2 
INDICATING UNITS OPEN 


RESET FLAGS FOR REMAINING UNITS 
INDICATING UNITS NOT OPENED 


DISPLAY LOGICAL ASSIGNMENTS TO 
USER AND PROMPT FOR FURG TO 

CONTINUE 


f 

i 
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1.1.2 LBRHELP 

Subroutine LBR_HELP is called to execute the help utility during a 
ROBSIM run. It uses the object file created from the macro HELPMAC.MAR 
and runs the system help utilities as required. 


(VCLR for LBR HELP not available.) 
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1.1.3 BLDARM 


OR’iG.'iv/;*. FA(2/I !3 

OF POOR QUALITY 

BLDARM is met when a selection of 1, to create or modify an arm data 
file, is entered from INITDRVR. The user choices for mode of operation 
are (1) create a simple cylinder arm data file, (2) modify existing arm 
data file, (3) enter detailed graphics data for arm (a simple cylinder 
file must already exist), or (4) terminate arm definition and return to 
the INITDRVR. For initial creation, option (1), subroutine ZERCOM is 
called to zero the COMMON locations and then CREATARM is called to build 
the new data file. For modification, RDARM and CREATARM are called when 
option (2) is requested. BLDDAT is responsible for the invention of de- 
tailed and geometry. In all cases, WRTARM will be called to write the 
arm data COMMON information. 
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1.1.4 BLDENV 

The user has the capability with routine BLDENV to specify a detailed 
physical representation for the robotic environment. Components for 
the environment are defined as basic geometric shapes (cylinders, 
cones, rectangular solids, symmetric or nonsymmetric trapezoidal fig- 
ures, triangular cross-sectional beams, rectangular beams, fillet com- 
ponents, data tablet-defined entities, obstacles, and CAD/CAM objects. 
The component type is written to the detailed graphics save file if 
requested. 
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SUBROUTINE BLDENV 
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1.1.5 BLDLOD 

Through routine BLDLOD, the user has the capability to specify a de- 
tailed physical representation for the robotic load objects to be 
used. Components for the load objects are defined as basic geometric 
shapes (cylinders, cones, rectangular solids, symmetric or nonsymmetric 
trapezoidal figures, triangular cross-sectional beams, rectangular 
beams, fillet components, data tablet-defined entities, nonplanar enti- 
ties, and CAD/CAM objects). This subroutine creates a new file, or 
modifies an existing file of load objects, and includes the capability 
to specify the detail at the first creation session for the load ob- 
jects. The component type is written to the detailed graphics save 
file if requested. 
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1.1.6 8LDSYS 

BLDSYS prompts the user for the moving base information and then reads 
individual arm data files into the appropriate system COMMON blocks. 

It desired, the locations of the fixed bases can be modified. BLDSYS 
then sets up the environment, load objects, target data, and stores the 
system file. 
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1.1.7 BLDTR6 


Through routine BLDTRG, the user can specify a detailed physical repre- 
sentation for the robotic target objects to be used. Components for 
the target objects are defined as basic geometric shapes (cylinders, 
cones, rectangular solids, symmetric or nonsymmetric trapezoidal fig- 
ures, triangular cross-sectional beams, rectangular beams, fillet com- 
ponents, data tablet-defined entities, obstacles, CAD/CAM objects and 
four-dot target patterns). This subroutine creates a new file or modi- 
fies an existing file of target objects, and includes the capability to 
specify the detail at the first creation session for the target objects. 


B-26b 



ORIGINAL. pv< - Rev A * October 1985 

*■ *** QUAurr 



B-26C 



Rev A, October 1985 


1.2.1 ZERCOM 

Subroutine ZERCOM is called from BLDARM, BLDLOD , and BLDTRG to ini- 
tialize the arm, load, and target data COMMON blocks prior to creating 
new system files. The COMMON blocks initialized to zero include: 

1) BLDGBK - arm geometric properties; 

2) BLDMBK - arm mass properties; 

3) GRAFBK - arm graphics data; 

4) BLDABK - arm actuator parameters; 

5) BLDSJBK - special joint flags; 

6) LOADBK - load geometry and mass properties; 

7) LGRAFBK - load graphics data; 

8) TARGBK - target geometry and mass properties; 

9) TGRAFBK - target graphics data. 



Rev A, October 1985 
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1.2.2 DEFUNIT 


Subroutine DEFUNIT is called during system definition to set up input 
and output units specified by the user. If these I/O units are not met- 
ric, the routine establishes conversion factors between the I/O units 
and internal (metric) units. The conversion factors are stored in vari- 
able CONUNIT, and LISUNIT contains a character string listing the I/O 
units employed. 



I 
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1.2.3 CREATARM 

Subroutine CREATARM is called within the system definition function to 
provide control of the creation or modification modes for the simple 
cylinder arm data file. The basic routines called for either option 
are SPAN (define arm span), BASE (define base geometric properties), 
BASELK (define base mass properties), JOINT (define joint), ACTUATOR 
(optional, to define motor properties), LINK (define link properties), 
and DEFSPJT (optional, to define special joints). If the user opts for 
an end-effector, TOOLJT (define tool-joint properties) and TOOLLK 
(define tool-link properties) are called. Graphics may be requested 
during CREATARM. 
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1.2.4 RDAJRM 


Subroutine RDARM is called from BLDAfiM to read from an unformatted arm 
data file the data describing a single arm. Routines called by BLDARM 
can then modify the data. The user is prompted for the name of the arm 
data file to be modified. The following data are read from it: 

1) 

Input/output units; 


2) 

Geometric properties; 


3) 

Mass properties; 


4) 

Graphics data; 


5) 

Actuator data; 


6) 

Information on special joints# 

The 

user has the option of 

saving or deleting the old data file# 


SUBROUTINE RDRRM 


PROMPT 

FOR NRME OF RRM DRTR FII_E 





OPEN 

RRM DRTR FILE 



RERD 

INPUT/OUTPUT UNITS DRTR 


read geometric properties drtr 



RERD 

MRSS 

PROPERTIES 

DRTA 





RERD 

GRRPHICS DRTR 





RERD 

RCTURTOR DRTR 


READ SPECIAL JOINT DRTR 

DISPLAY 

MESSAGE THAT Al_l_ DATA HAS BEEN READ 


T\v 




SRVE 

ARM DRTA FILE 

/F 

— * 7 

U-I 

cn 

cn 

U-J 


T \ 

DELETE 

ARM 

^ 

/* 

cn 

> 

L_i_J 

1 1 

Lj 

CO 


DELETE FILE 

CLOSE RND i 

SRVE FILE 1 

i 

i 


RETURN 


END 
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1.2.5 BLDDAT 

Subroutine BLDDAT provides the user the capability to specify a more 
detailed physical representation for the links of the robotic arm. 
Components of the robotic arm system are defined by combinations of 
geometric primitives. A number of detailed components can be included 
for the base, each link extension and the tool definitions. The com- 
ponents are simple three-dimensional shapes: the cylinders, cones, rec- 
tangular solids, symmetric trapezoids, nonsymmetric trapezoids, trian- 
gular cross-sectional beams, rectangular beams, data tablet structures, 
fillet components, nonplanar entities, and CAD/CAM objects. Unique 
subroutines are called to handle loading the graphics object data for 
the shapes chosen to represent a detailed arm. Additional shapes can 
be added as required. The component type is written to the detailed 
graphics save file if requested. 
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1.2.6 WRTARM 


Subroutine WRTARM is called from BLDARM to save, in a user-specified 
file, the data generated when creating or modifying an arm description. 
The user is prompted for the name of this file and is also given the op- 
tion of storing a formatted file containing the arm description for 
later printing. 


SUBROUTINE URTflRM 


PROMPT FOR NRME OF FILE TO WRITE RRM DRTR TO 


OPEN FILE 


WRITE FORMATTED RRM GEOMETRY FIl-E FOR PRINTING 


PROMPT FOR NRME OF RRM GEOMETRY PRINTOUT FILE 


OPEN FIL-E 


WRITE RRM GEOMETRY DESCRIPTION TO FILE 


CLOSE RNO SAVE FILE 


WRITE INPUT /OUTPUT UNITS DRTR TO FILE 
WRITE RRM GEOMETRY DRTR 


WRITE RRM MRSS PROPERTIES 


WRITE RRM GRRPHICS DRTR 


WRITE RCTURTOR DRTR 


WRITE SPECIAL UOINT DRTR 


DISPLAY MESSRGE THAT FIl_E WRS WRITTEN 


CLOSE RNO SAVE FILE 


RETURN 

END 




1.2.7 ENVIR 


Subroutine ENVIR interactively establishes the basic properties of the 
system environment during system definition. This includes the input/ 
output units, the gravity vector and the system span. 


SUBROUTINE ENVIR 


PROMPT 

USER 

FOR GRRVITY FLAG. STRNDRRD 
OR INPUT VRI-UE 

M 

RERD USER RESPONSE 

Ss 


USER REQUESTED STRNDRRD 
GRRVITY 



§ *=* 


PROMPT USER FOR INPUT RCCELERRTION 
DUE TO GRAVITY IN DEFAULT UNITS 


12 


RERD USER INPUT GRRVITY VECTOR INTO 
GRRV PRRRMETER 



CALL CVTUNIT GRRV TO CONVERT 
TO INTERNAL MRTH UNITS 


PROMPT USER FOR SYSTEM SPRN IN 

DEFAULT UNITS 


READ USER INPUT INTO SYSSPN 

PRRRMETER 


CRL.L. CVTUNIT TO CONVERT SYSSPN TO 
INTERNAL MATH UNITS 


RETURN 


END 
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1.2.8 RDENVS 


The subroutine RDENVS is called from BLDSYS if the user wishes to in- 
clude an environment in the system being created. This routine reads 
the unformatted environment data file created by the system definition 
function for the multiarm system. The user is prompted for the name of 
the data file under which the environment data have been stored. The 
file is opened and COMMON block ENVTBK loaded from the data file during 
system creation. The file is closed and saved. 


SUBROUTINE RDENVS 

PROMPT FOR NAME OF FILE CONTAINING 

ENVIRONMENT DATA __ 

OPEN ENVIRONMENT DATA FILE 


READ input/output units data 

READ GRAPHICS DATA FOR ERCH 

COMPONENT 

DO UNTIL N - NUMBER OF COMPONENTS 
IN ENVIR ONMENT 

CLOSE FILE AND SAVE 

DISPLAY MESSAGE THAT FILE HRS BEEN 

RERD RND SAVED 


RETURN 



1.2.9 RDARMS 

Routine RDARMS is called during the total robotic system creation for 
each of the arms desired for inclusion in the system setup. The sub- 
routine RDARMS reads the unformatted data file created by the system 
definition function containing any one arm file. The user is prompted 
for the name of the data file under which the arm data have been stored. 
The file is opened and read into the following COMMON blocks: GEOMBK, 
AMASBK, IOBJBK, TOOLBK, FORCBK, MOTORBK and SPJTBK. 


ORIGINAL = - 
OF POOR QUALITY 


SUBROUTINE RDRRMS 


SETT PROCESSSOR MODE - 1- FOR SYSTEM DEFINITION 

PROMPT USER FOR FILENAME OF SINGLE RRM FILE TO RERD 

OPEN RRM DRTR FILE 

STORE SYSTEM UNITS IN TEMPORRRY RRRRYS 

RERD UNITS COMMON BLOCK INTO SYSTEM COMMON 

RCRO GEOMETRY COMMON BLOCK FOR BASE. JTS. RNO TOOL INTO SYSTEM 

COMMON 

READ MASS PROPERTIES COMMON BLC3CK FOR BASE* JTS. AND TOOL- INTO 

SYSTEM COMMON 

CONVERT PRORCIITiea TO INTERNAL UNITS TOR SLXOXNS RNO ROTRTINg | 

-ITS. 

PUT TOOL NH33 PROPERTIES INTO SYSTEM TOOL COMMON BLOCK 

RERO GRAPHICS DRTR COMMON BLOCK INTO SYSTEM COMMON 

CONVERT RRM SPAN TO INTERNRL UNITS 

SCALE GRAPHICS OBJECT DATA BY (ARM SPAN / SYSTEM SPAN! 

READ ACTUATOR DRTR COMMON BLOCK INTO SYSTEM COMMON 

CONVERT RCTURTOR DRTR TO INTERNRL UNITS 

REPO SPECIAL JOINT DATA COMMON BLOCK INTO SYSTEM COMMON 

CLOSE SINGLE RRM DRTR FILE 

REWRITE SYSTEM UNITS INTO SYSTEM UNITS COMMON BLOCK 

RETURN 

END 
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1.2.10 BASES 


BASES modifies Che base location or orientation when including an arm 
in a system; it is called from BLDSYS. 



• SUBROUTINE BASES 

SET MODE FL-RG - 1 

UPTTE CURR ENT BRSE LOCATION VRUUE TO TERMINRL 

PROMPT FOR BASE LOCRTION MODIFICA TION FL-RG 

INPUT MODIFY FLOG - 2 

ro w New x. v. ■ location or 3N kwj eoowoa CNLJLJ—3 

Rgffo xTyTi i _oorrxoN of mcmm. xnto ftjTuoc nonm i«. t *im 

URTTE CU RRENT BRSE ORXENTION VALUE TO TERMINAL 
PROMP T FOR BRSE ORIENTATION MODIFICATION FURG 

INPUT modify flrg » 2 

' I - o 

DO UIHXL-E X . I— T . 3 . — . — 

I X = x •+• 1 

PROHET FOR I TH ROTBTTON agOUCNCg WXX3 OP ROTHTXON 


Ipepo rotation 


rrj. »NCg AXIS or ROTATION XNTO XPOT tXl 



PROMPT FOR X TH ROTATION ANGLE 

PERD R OTBTTON PNGL-g XNTO Q-ITPMQ FOR X TH SgOUgNCg NUHBgW 

PRO MPT FOR USER INPUT TERMINATION 
□□ ijimtti I IPPP TERMINATES rotrtxon sequence input 



npK -aTgN BTE NEW WOT MPTPXX (TgMPll MXTH C U BPgNT ROT MPTPXX tTEMPi _ 
^^OTPTXON MATRIX. OP. UXTN BESuC tXNG TR^SF o PMED MPTPXX 
RETURN 




1.2.11 SETUP 


Subroutine SETUP calls SETUP2 for each arm in the manipulator system to 
calculate the positions of all arm components in terms of world coordi- 
nates. 


SUBROUTINE SETUP 



CALL SETUP2 TO CALCULATE ALL 
POSITIONS IN WORLD COORDINATES 

DO UNTIL. KRRM - NUMBER OF RRMS 
THE SYSTEM 

IN 


RETURN 



END 



I 

I 
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1.2.12 SETUP2 

SETUP2 works every increment. It calculates the positions of all links 
(including base, tool, and any held loads) and transforms link and cen- 
troid vectors to world coordinates. The recursive positioning method 
described in the main text is used. Finally, subroutine JACOB is called 
to compute the Jacobian for the current position. 


SUBROUTINE SETUP2 



B— 38 
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1.2.13 HARD_COP 

Subroutine HARDjCOP is executed when a hardcopy record of the current 
Evans and Sutherland display may be desired. This routine queries the 
user to determine if a hardcopy is desired and runs the appropriate rou- 
tines to create a picture file for later translation into a hardcopy 
plot. 


(VCLR for HARD COP is not available.) 
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1.2.14 SYSGRAF 

Subroutine SYSGRAF provides the system definition graphics capability 
in the system definition function. SYSGRAF displays the environment, 
target, load and robotic arm choices for building a robotic system 
scenario. It takes as input through the calling sequence, the number 
of arms in the system, a flag indicating the existence of an environ- 
ment file for the system, a target file, and a load objects file inclu- 
sion indicator. It uses the system span input by the user to scale the 
graphics picture. IFLAG controls the logical flow in the subroutine. 

If IFLAG=1, the graphics system is initialized and displayed in the 
initial condition; if IFLAG =2, the robotic system, targets, loads and 
environment are displayed; if IFLAG=3, the graphics display is termi- 
nated. In the update mode, the environment is constant and therefore 
not updated. As before, the Evans and Sutherland graphics routines are 
used to provide all graphic capabilities. 
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15 RDLODS 


RDLODS reads in load daCa if the user requests that loads be included 
in the robotic system under construction. The file read is the unfor- 
matted load file created by the system definition function. Subroutine 
RDLODS prompts the user for the name of the file containing load data, 
and then reads those data into COMMON blocks LGRAFBK and LOADBK during 
system creation. The load file is scaled from load units to internal 
system units, closed, and saved. 


SUBROUTINE RDLODS 


PROMPT FOR NOME OF FII_E CONTAINING LORDS 

DRTR 

LORDS DRTR FILE EXISTS 

OPEN LORDS DRTR FILE 


STORE SYSTEM I/O UNITS IN DUMMY 

VARIABLES 

RERD LORD OBJECTS INPUT/OUTPUT UNITS 

DRTR 

RERD LORD MRSS PROPERTIES DRTR 

\ LORO OBJECTS I/O UNITS NOT / 

T\ METRIC / F 

=j 

CONVERT LORO VRRIRBUES TO riSJLJI 1 1 

XNTCRNRU CMCTRXC3 UNITS IPMLJL-L-J 


RERD LORD OBJECTS GRRPHICS DRTR 

CLOSE RND SRVE FILE 

RESTORE SYSTEM I/O UNITS 

RETURN 

END 
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1.2.16 LOCMOD 


Subroutine LOCMOD is called from BLDSYS to allow the user to modify the 
locations and orientations of load objects when building a system. The 
current location is displayed and then the user is prompted for a new 
location. The subroutine also displays the transformation matrix for 
the current orientation of the load object and prompts the user for a 
sequence of rotation axes and angles that define a change in orienta- 
tion. ROTMAT is called to calculate the transformation matrix from the 
user input, and MATMPY combines this new transformation matrix with the 
old one. 


SUBROUTINE LOCMOD 



DISPLAY CURRENT LOCATION OF 

OBJECT 

LOAD 



PROMPT USER FOR AND READ IN 
LOAD OBJECT LOCATION 

NEW 


□ISPLRY TRANSFORMATION MATRIX FOR 
CURRENT ORIENTATION OF LOAD OBJECT 


MODIFY ORIENTATION 

V 


£ 


USER INPUT OF A ROTATION 

AXIS 



USER INPUT OF ROTATION 

ANGLE 


DC 

RC 

UNTIL USER STOP FLAG OR 3 
JTATIONS 


CALL ROTMAT TO CALCULATE NEW 
TRANSFORMATION MATRIX 


CALL MATMPY TO COMBINE OLD AND 
NEW TRANSFORMATION MATRICES 


RETURN 

END 
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1.2.17 WRTSYS 

Subroutine WRTSYS is called from BLDSYS to write an unformatted file 
containing the data needed to describe a system. These date include 
user units, moving base flags, arm geometry, mass and actuator proper- 
ties, special joint data, arm graphics, environment, load object and 
target properties, tool data, and load object and target graphics 
information. 
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1.2.18 RDENV 


The subroutine RDENV reeds an unformatted environment data file during 
the system definition function. The content of the file is the perti- 
nent COMMON block defining an environment for the robotic system. The 
user is prompted for the file name from which the file is to be read. 


SUBROUTINE RDENV 

PROMPT USER FOR FILENAME OF 
ENVIRONMENT FILE TO RERD 

OPEN ENVIRONMENT DATA FILE 


RERD UNITS COMMON BLOCK 

RERD ENVIRONMENT GRRPHICS DRTR 

COMMON BLOCK 


CLOSE ENVIRONMENT DRTR FILE 


RETURN 


END 





ORIGINAL PAS'- i3 

1.2.19 DRWENV OF POOR QUALJTY 

DRWENV is called within the system definition function from BLDENV to 
provide graphics display during the generation of detailed environment 
graphics representations. It is called to display each successive en- 
vironment component as it is defined. 



SUBROUTINE 

DRWENV 



|SET PROCESSOR MODE — 1. FOR 

SYSTEM DEFXNXTXON 



B8I 

INITIALIZING 



PROMPT USER POP ENVIRONMENT SPRN 

CNUL.LU 



SET SCRLE FRCTOR. IFRCT — 

lOOO. /ENVIR. SPRN 



SSI 

INITIALIZING 

DISPLAY 


si 



SET WINDOW BOUNDARIES 


3ZM.8 


K 

DRAWING OR REPLACI 

NG COMPONENT 



SET WINDOW BOUNDARIES 


SET 

CURRENT INTEGER TRRNSI— RTXON RNO ROT. VALUES 



SET 

NUMBER OR COMPONENTS IN ENVIR. PARAMETER 



DO 

FOR ALL COMPONENTS BEFORE CURRENT COMP. 



SET 

OOTPWXCS PLOTS POR SSQUSNTSAL RNO 

RUTBRNRTXNG PTS. 

1 



i CALL D3DRTR TO DISPLAY 

COMPONENT 


— ! 

SET 

COUNTER FOR LP8T ENV. O0JKCT RR P 

«Y LOCATION USED 



SET 

snnn IXC 3 PLOTS POR SEQUENTIAL AND ALTERNATING PTS. 



CALL D3DATA TO DISPLAY COMPONENT 


SET 

COUNTER FOR LAST ENV* OBJECT ARRAY LOCATION USED 



CLOSE AND REPLACE SEGMENT 


RETURN 

END 


i 

i 

f 

i 

i 

i 
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1.2.20 WRTENV 


Subroutine WRTENV writes an unformatted environment data file during 
the system definition function. The content of the file is the per- 
tinent COMMON block defining an environment. 


SUBROUTINE WRTENV 


PROMPT USER FOR FILENAME OF 
ENVIRONMENT FILE TO WRITE 


OPEN ENVIRONMENT DRTR FILE 


WRITE UNITS COMMON BLOCK 


WRITE ENVIRONMENT GRAPHICS DRTR 

COMMON BLOCK 


CLOSE ENVIRONMENT DRTR FILE 


RETURN 


END 
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1.2.21 RDLOAD 


The subroutine RDLOAD reads an unformatted load objects data file during 
the system definition function. The contents of the file are the per- 
tinent COMMON blocks defining a load file for the robotic system. The 
user is prompted for the file name from which the file is to be read. 


SUBROUTINE RDLORD 


PROMPT USER FOR FILENRME OF LORD 
OBJECTS FILE TO RERD 


OPEN LORD OBJECTS DRTR FILE 


RERD UNITS COMMON BLOCK 


RERD LORD OBJECTS MRSS PROPERTIES 

COMMON BLOCK 


RERD LORD OBJECTS GRAPHICS DRTR 

COMMON BLOCK 


CLOSE LORD OBJECTS DRTR FILE 


RETURN 


END 
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1.2.22 LOAD 

Subroutine LOAD is called during the BLDLOD option of INITDRVR. It 
allows the user to create and define the mass properties of one or more 
load objects. If a file of load object data already exists, this sub- 
routine may be used to modify portions of those data. The load para- 
meters for which the user is prompted are listed: 

1) Load object name (up to 8 characters); 

2) Location and orientation with respect to the world coordinate sys- 
tem; 

3) Length and radius; 

4) Center of mass; 

5 ) Mass ; 

6) Inertia distribution; 

7) Grasp point and approach vector in load local coordinates; 

8) Mass and location of any point masses included. 
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1.2.23 TLDMAS 


Subroutine TLDMAS is 
masses included in a 
location, and rotary 
simple load object, 
calculate new values 
effects. 


called from BLDLOD to add the effects of point 
load object. Variables for total mass, centroid 
inertia are initialized with the values for the 
If point masses are included, ADDMAS is called to 
for these variables that include the point mass 


SUBROUTINE TLDMRS 


INITIALIZE TOTAL LOAD MASS. 
RND INERTIA WITH SIMPLE LORD 

CG. 

DRTR 



T 

\ NUMBER OF POINT MRSSES FOR 

N. CURRENT LORD OBJECT . GT. O 


/ - 

1 

CALL RDDMRS TO 
INCORPORATE POINT 
MASS DATA 



— . 


□O UNTIL IP - NUMBER 
OF POINT MASSES FOR 
CURRENT LORD OBJECT 





RETURN 


END 
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ORIGINAL PAGE IS 

1.2.24 drwlod ^ POOR QUALITY 

DRWLOD is called within the system definition function from BLDLOD to 
provide graphics display during the generation of a detailed load ob- 
jects file. It is called to display each successive load object compo- 
nent as it is defined. 


1 SET "PROCESSOR MODE - 1. FOR SYSTEM DEFINITION. 

INITIALIZING /f 



^ 


ZERO INTEGER 
TRRNSUATXON RND 
ROTATION VRUUES 

3ET I ~ 

r~oi |' CISLS TO STATUS RNPUOG CONTROL OIRLS J 

flFT “RENT INTEGER TRANSLATION fiNO HOT. VRUUES 1 

DRRUJXN 

G OR REPLRCING COMPUNtN i 



SET UINDOUI BOUNDARIES 

SET CURRENT INTEGER TRANSLATION RNP WOT. VALLES 


jp RPU COLOR COOROXNRTEO AXES SYSTEM tREP- X. HHXT-Y. Bl-U-C 
ggj NUMBER - OF COMPONENTS IN L.QRO PARAMETER 


uu 

SET 

r ur^ i 
GRAPHICS 

FURG3 FOR 

seoueNTxnu 

AND SUTERNRTXNG 

PTS. 


CRl— 1— 

D3DRTR 

TO DISPLAY COMPONENl 


SET 

COUNTER FOR LAST LOAD OBJECT 

ARRAY LOCATION 

USED 


HALL D3DRTR TO DISPLAY COMPONENT 


SET COl INTER FOR LAST LORO OBJECT ftWRRV LOCATION USED 

CLOSE RND REPLRCE SEGMENT 



(NULL) 



1.2.25 WRTLOD 


Subroutine WRTLOD writes the unformatted load objects data file during 
the system definition function. The contents of the file are the per- 
tinent COMMON blocks defining the mass properties and graphics of the 
loads for a system. 


SUBROUTINE WRTLOD 


PROMPT USER FOR FILENAME OF LOAD 
OBJECTS FILE TO WRITE 


OPEN LORD OBJECTS DATA FILE 


WRITE UNITS COMMON BLOCK 


WRITE LORD OBJECTS MRSS PROPERTIES 

COMMON BLOCK 


WRITE LORD OBJECTS GRAPHICS DRTR 

COMMON BLOCK 


CLOSE LORD OBJECTS DRTR FILE 


RETURN 


END 
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1.2.26 RDTRGS 

RDTRGS reads in target data if the user requests that targets be in- 
cluded in the robotic system under construction. The file read is the 
unformatted target file created by the system definition function. 
Subroutine RDTRGS prompts the user for the name of the file containing 
target data, and then reads those data into common blocks TGRAFBK and 
TARGBK during system creation. The target file is scaled from target 
units to internal system units, closed, and saved. 
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1.2.27 LOCTMOD 

Subroutine LOCTMOD is called from BLDSYS to allow the user to modify 
the locations and orientations of target objects when building a sys- 
tem. The current location is displayed, and then the user is prompted 
for a new location. The subroutine also displays the transformation 
matrix for the current orientation of the target object and prompts the 
user for a sequence of rotation axes and angles that define a change in 
orientation. ROTMAT is called to calculate the transformation matrix 
from the user input, and MATMPY combines this new transformation matrix 
with the old one. 
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1.2.28 RDTARG 

The subroutine RDTARG reads an unformatted target object data file 
during the system definition function. The contents of the file are 
the pertinent common blocks defining a target file for the robotic sys- 
tem. The user is prompted for the file name from which the file is to 
be read. 
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1.2.29 TARGET 

Subroutine TARGET is called during the BLDTRG option o£ INITDRVR. It 
allows the user to create and define the properties of one or more tar- 
get objects. If a file of target object data already exists, this sub- 
routine may be used to modify portions of these data. The target para- 
meters for which the user is prompted are the location and orientation 
with respect to the world coordinates. 
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SUBROUTINE 


Rev A, October 1985 
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1.2.30 DRWTRG 

DRWTRG is called within the system definition function from BLDTRG to 
provide graphics display during the generation of a detailed target ob- 
ject file. It is called to display each successive target object com- 
ponent as it is defined. 


Rev A, October 1985 
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1.2.31 WRTTRG 

Subroutine WRTTRG writes the unformatted target object data file during 
the system definition function. The contents of the file are the per- 
tinent common blocks defining the mass properties and graphics of the 
targets for the system. 
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SUBROUTINE 
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1.3.1 GRINIT 

For building an environment, simple arm, detailed arm, targets or loads 
with graphics, routine GRINIT initializes the E&S display, extended 
switches/lights, and analog control dials, and draws the graphics dis- 
play border. Working from the input flag IFLAG, the type heading of 
the general display is chosen, either simple cylinder, detailed geome- 
try, environment, target or load. The graphics segments are opened and 
the title for the system definition function driver currently under 
execution is output. 
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1.3.2 SPAN 


The manipulator arm span is requested as input from the user during ini- 
tial creation of simple cylinder arm data; modification of the ARMSPN 
value is also allowed through a call to SPAN during the CREATARM modifi- 
cation mode. 


SUBROUTINE SPAN 



ERROR CODE = 

O 



T 

SYSTEM MODIFICATION 
\ SET 

FLAG 

IS 


PROMPT FOR SPRN 
VALUE MODIFICATION 
FLRG 

CNULLD 

N 

T 

^ INPUT MODIFY FLAG 

. NE. 

1 



PROMPT FOR NEW SPRN. 
OR REACH* OF ARM 


READ SPAN -INTO RRMSPN 
PARAMETER 

RETURN 


END 


CNULL] 
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1.3.3 BASE 


ORIGINAL PAGE IS 
OF POOR QUALITY 


Subroutine BASE is called within the system definition function during 
definition or modification of the simple cylinder arm or detailed arm 
geometry file. The purpose of subroutine BASE is to provide the input 
of the robotic base position, orientation, and physical dimensions 
(radius of base, endpoints and number of sides), and to load these 
values into COMMON blocks. 


SUBROUTINE BASE 


SET MODE Fl_flG 


SET ERROR CODE FLRG 


ZERO ROTATION SEQUENCE MATRIX 




PROMPT FOR BASE MODIFICATION DESIRED 


READ MODIFICATION CATEGORY DESIRED 


















ORIGINAL PAGE IS 
OF POOR QUALITY 

1.3.4 OBJECT 

Subroutine OBJECT creates simple cylinder graphics data used by the gra- 
phics package to draw the robotic arm during the system definition func- 
tion. The data created in OBJECT are stored in COMMON block IAKMOBJ, 
and represent a right circular cylinder of the specified size for each 
system component (the base, each link and the tool). It is called for 
generation of each of these components in turn. 



i 

C 

I 
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1.3.5 GRAPH 

If the graphics are initiated during the simple single arm creation or 
modification, subroutine GRAPH provides the graphics capability for the 
simple cylinder representation of the robotic manipulator. GRAPH dis- 
plays each base, joint/link combination and tool as they are defined. 
Graphics during the modification mode is handled with calling arguments 
input to GRAPH; appropriate deletions, additions and changes to the 
links are visually depicted. GRAPH provides only the simplified robotic 
arm definition display. 


SUBROUTINE GRAPH 


5CT SGALjE FACTOR* 3TRCT — lOOO» /ARM 3PWN 
«f"r PTCTURg PROCgaaOR TRRN3. TO IDENTITY . 


r pTPL-3 TP 3TftTU3 RNfflOQ CONTROL OXRL3 
jjgT TNTgQgR TWRNaU*TXQM ffNO ROTATION VALUER 


SCRL.C JT, LOCATION BY irACT AND LORO INTO INTEGER RRRRV 
'CRI-I- TTRRN TOR PICTURE PROCESSOR TRANSFORMATION MATRIX 

EXTRACT OFFSET >JT, ANG. FROM JOINT VARIABLE FARRffV 

Cfli_L. T ROTX OR —V LlXTM JT, ANG. TO ROTATE TRANSFORMATION 

EXTRACT R OTATION AXES USED XN ORIENTING. FROM JOINT SgQU CNCg ffRRRV 
EXTRACT X* Y» Z RO T. ANGLES UggP XN ORIENTING. i^HOH JT, R NSUg ARRAY 
CR| _ L T ROTX. ~Y OR -Z UIXTH INTEGER ANG. TO ROTATE TRANSFO RMATION 
SET number or COMPONENTS xn link parameter 

DO FDR EAC H ZOMPONENT XN LINK 

3gT sfjRRT LOCATION XN ARM OBJECT ARRAY FOR CURRENT COMPONgN T 

LOAD ARM OBJ. ARRAY FOR GRAPHICS FLAGS 

L ORD ARM OBJ. ARRAY FOR SEQUENTIAL POINTS 

L OAO ARM OBJ. "ARRAY FOR ALTERNATING POINTS 

rot.L nanRTR to dis play link 

gg-r t-nilSi TgR FOR’ LAST ARM OBJECT ARRAY LOCATION USES 


DO UNTZL. «_U gXXSTZNQ L-XNK3 HAVE BggN DARAN 




1.3.6 DIALS 


DIALS is called Co scale Che Evans and SuCherland analog conCrol dials 
values read during camera perspecCive changes via Che exCended E&S 
dials. The values are scaled Co inCegers beCween -32767 and +32767. 


SUBROUTINE DIALS 


TRANSFER GRAPHICS ANRLOG CONTROL 
DIALS DEVICE RECORD FROM QUEUE 


DO FOR ERCH CONTROL DIAL VALUE RERD 


XI 

VALUE . GT. 

32‘767L 



— 

SET VALUE TO 
-65534 -h VALUE 

CNULLi 

X 

i T X 

VALUE - LT. 

-32767. 



SET VALUE TO 
65534 *+* VALUE 

C NULL. 3 


RETURN 


END 
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1.3.7 JOINT 

For JOINT, the user inputs joint type, joint location as Cartesian coor- 
dinates in terms of the coordinate system of the previous joint (or 
base, if the current joint is joint 1), joint orientation as a rotation 
sequence of axes and corresponding angles with respect to the previous 
joint coordinate system (or base, if joint 1), and initial joint state 
(initial angle for hinge or swivel, or initial length for sliding 
joints). The x-axis of a joint coordinate system is directed along the 
centerline of the link between joint i and joint i+1 (or end— effector 
if the current joint is the final joint in the system). JOINT is called 
by CREATARM during initial creation or modification of arm data. 
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1.3.8 ACTUATOR 


ACTUATOR allows the user to define or modify the COMMON blocks defining 
actuator properties for the arm by interactively prompting for actuator 
parameter values or by reading a previously constructed file of actuator 
parameter values. The user can opt for no actuator definition if 
desired. 


subroutine: rcturtor 
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1.3.9 LINK 


ORIGINAL PAGE IS 
OF POOR QUALITY 


In the create mode of LINK, the user is prompted for link endpoints in 
coordinates along the x-axis, link radius, the location of center of 
mass as the Cartesian coordinates of the center of gravity in the coor- 
dinate system of the joint at the "base" end of the link, link mass and 
inertia matrix relative to the centroid and the number of sides for the 
desired simple cylinder. 



c-> 
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1.3.10 DEFSPJT 


This routine interactively establishes the number, type and location of 
"special joints." These are joints for which a constraint is placed on 
the relative joint displacements. 


SUBROUTINE DEFSPJT 


PROMPT FOR NSPJT — THE NUMBER 
OF SPECIAL JOINTS IN THE ARM 

DO FOR EACH SPECIAL UOINT 

1 

PROMPT FOR TYPE OF SPECIAL 
UOINT AND SET ISPTYP 


PROMPT FOR WHICH UOINT OF 
ARM THE SPECIAL UOINT IS AND 

SET NUTSP 
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1.3.11 TOOLJT 


Subroutine TOOLJT is called by CREATARM to interactively define or modi- 
fy the geometry properties of the manipulator end-effector. The data for 
which the user is prompted include: 

1) Location of tool with respect to final link; 

2) Orientation of tool with respect to final link. 

The orientation data are input as a sequence of rotations about coordi- 
nate axes and ROTMAT is called to compute the corresponding rotation 
matrix. 


SUBROUTINE TOOLJT 


INITIALIZE ROTATION SEQUENCE CIROT3 
TO ZERO INDICATING NO ROTATIONS 

t\ 

MODIFYING PREVIOUS DRTR 


cr: or 

SUCCESSIVELY PROMPT FOR X. Y. RNO 


CD Li_ 

H LOCATION OF TOOL WITH RESPECT TO 


LU 

1 CO 

rr " 5 *^ 


PRECEDING LINK 


U O >- 
H 1 


IR - O 


i 




CD CD CD 
M CX 

■ 

IR = IR+ 1 


ul CO QZ 
CD I CD 




H-H CD CD 

t i ri lit 

HR 

ENTER IROT CIR3 - COORDINATE AXIS 



; ££ 

FOR ROTRTION 


CD M CC 

'■*v ■ ' ' 1 



* — ) t \ 




CC I 

rr- t — i CH 

y 

ENTER AvJTRNG CIR3 — ANGLE OF 


C=> UL- 2Z 


ROTATION 


Cl— QZ H— 
U-J 
) CDl_ 

■ 

ABOUT THIS AXIS 


i nL ■ 

DC CD 
O 21 
CC CC 

r-i 

1 

PROMPT FOR USER TERMINATION 


1 1 *■ ii 

CO 

LU 

DO 

UNTIL USER TERMINATES OR IR - 3 

1 1 — 

i — i cn 

SET JNTSEO - IROT Cl 3 IROT C23 IROT C33 

21 

— 

1 

Z) CC X 

CALL ROTMflT TO COMPUTE ROTATION MATRIX 

i i i rr 
C3 CO LxJ 

CORRESPONDING TO THIS ROTRTION SEQUENCE 

=» = * 



. - - -1 
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1*3*12 TOoLLK 


ORIGINAL Pfiqc ,~ 

0F POOR QUAUr': 

- — - ■ i 

Subroutine TOOLLK is called by CREATARM to Interactively define or modi 
fy the mass and graphics properties of the manipulator end-effector* 

The data for which the user is prompted include: 

1) Endpoints for cylinder representation; 

2) Radius of cylinder; 

3) Number of sides of cylinder; 

4) Center of mass of end-effector; 

5) Mass; 

6) Inertia distribution; 

7) Location and mass of point masses* 

When modifying existing data, the user has the option of which cate- 
gories to modify* 



RETURN 


ENC 
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1.3.13 TOTMAS 


Routine TOTMAS combines individual components of each link's contribu- 
tions (e.g., link mass, point masses) to obtain a total mass distribu- 
tion for the joint/link combinations and tool during robot arm creation. 
Variables for the total mass, centroid location, and inertia distribu- 
tion are initialized with the values from the simple link. If point 
masses are included, ADDMAS is called to add the effects of these addi- 
tional terms. 


SUBROUTINE TOTMRS 

INITIFiLlEE TOTAL MASS. C- G. - RND 

inertir variables with link data 


T 

NUMBER OF POINT MRSSES 
FOR CURRENT LINK . GT. O 

Z 



CRLL RDDMRS TO 
INCORPORRTE POINT 
MRSS DRTR 




□Q UNTIL IP ** NUMBER 
OF POINT MRSSES FOR 
CURRENT LINK 





DO UNTIL N = NUMBER OF LINKS IN RRM 


RETURN 


END 
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1.3.14 ADDMAS 


Subroutine ADDMAS combines the mass properties of two objects to obtain 
composite values for the mass, centroid location, and inertia distribu- 
tion. ADDMAS calls ADDMAS2 to perform the computations. ADDMAS then 
loads the results into the first object's mass property variables. 


SUBROUTINE ROOHRS 


CRL.L. RDDMRS2 FOR COMPOSITE INERTIR 
RNO PUT XN TEMP VRRIflBLES 


PUT MASS RESULTS INTO MASS OF BODY 

1 


PUT C- G. RESULTS INTO C. G. OF BODY 

1 


PUT INERTIA MRTRXX INTO XNERTXR 
MRTRXX OF BOOY 1 


RETURN 


END 







1.3.15 ADDMAS2 


ADDMAS2 calculates the composite mass properties of two rigid bodies 
joined together. The mass, centroid location, and inertia matrix for 
the composite body are returned as results. 


SUBROUTINE RDDMRS2 


TOTRL MASS =* MRSS OF BODV 1 MRSS 

OF BODV 2 


COMPOSITE CENTROID — CMRSS 1 CG 

1 MRSS 2 CG 23 / TOTRL MRSS 


R 1 = CG 1 — COMPOSITE CENTROID 


R2 = CG 2 — COMPOSITE CENTROID 


CALL RCICR FOR R1 SQURRED MRTRIX (R 1 SQJ 
USED TO FIND COMPOSITE INERTIA 


CRLL RCICR FOR R2 SQURRED MRTRIX CR2SQ3 
USED TO FIND COMPOSITE INERTIR 


COMPOSITE INERTIR — RIN 1 -+• RIN2 ■+■ 

CMRSS 1 R1SQ ■+• MRSS 2 R2SQ3 


RETURN 


END 
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I 


1.3.16 RCXCR 

Subroutine RCICR is called by ADDMAS2 to set up the inertia matrix 
corresponding to a point mass displaced from the body centroid. This 
inertia matrix forms one component of the inertia distribution for the 
composite body. 



I 

t 


\ 
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1.3.17 GRTERM 




GRTERM is called to terminate the Evans and Sutherland device processor 
display unit. It calls MPSTOP to terminate the multi-picture processor 
display unit graphics. 
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1.3.18 BASPUT 


Subroutine BASPUT is called from subroutine SETUP2 during position 
calculations for the manipulator. This subroutine takes the position 
and orientation of the base of each arm (with respect to the world 
coordinate system) and loads these data into the arrays POS and ROT. 


SUBROUTINE BASPUT 



PUT ARM BASE LOCATION IN ARRAY 

POS 


1 

PUT 

ARM 

BASE ORIENTATION IN 
ARRAY ROT 

DO 

l UNTIL. 

U - 

3 


DO UNTIL. 1-3 

UNTIL. KARM - NUMBER OF ARMS 

RETURN 

END 








1.3.19 JACOB 


Subroutine JACOB sets up the Jacobian matrix that will later be used to 
solve for individual joint velocities for each arm given the end effec- 
tor velocity. This subroutine uses end-effector position and joint to 
world transformation matrices to determine the entries of the Jacobian 
as described in a previous section. The result is a 6xN matrix for each 
arm. 


SUBROUTINE JRCQB 


crll 

MRTMPY 

TO RUT REF POINT VECTOR IN 

UJORLO COORDINATES 

ROD 

VECTOR 

FROM UJORLO ORIGIN TO ENO 

EFFECTOR ORIGIN 



joint is hinge or swivel 


FIND DIRECTION COSINES OF JOINT 
AXIS U. R. T. WORL.D COOR. CR CSJ J 


CRU_ CRFO TO PINO RM - R X (VECTOR 
FROM END EFT. REF. PT. TO JOINT! 


RJRCOB Cl. JTD 


RW C13 


RJRCOB Cl. _m - 
ROT Cl. 1. JT. KRRM3 


RJRCOB C2. UT3 - 
ROT C2» 1. JT. KRRM3 


RJRCOB C2. UT3 — RW C23 
RJRCOB C3. UT3 — RW C33 
RJRCOB C-4. UT3 — R C13 
RJRCOB CS. UT3 - R C23 

RJRCOB CS. JT3 = R C33 
DO UNTIL. JT = NUMBER OF 


RJRCOB C3. UT3 — 
ROT C3. 1. JT. KRRM3 


RJRCOB C-4. JT3 

- o. o 


RJRCOB CS. JT3 

- o. o 


RJRCOB CS. JT3 

- O. O 

JOINTS IN RRM 


RETURN 

END 
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1.3.20 DATOUT 

DATOUT is responsible for the data output in the columns set up by sub- 
routine FORM of the E&S robotic simulation display. It includes the 
current simulation time, joint travel angles, percent of the maximum 
traveled for each joint and task commands. DATOUT has provisions for 
only two arms. 
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1.3.21 FORM 

The FORM routine sets up the borders and the text output locations for 
the manipulator display on the E&S graphics unit. It sets up the Evans 
and Sutherland graphics display borders and outputs the robotic simula- 
tion title, current simulation time text title, joint travel status 
data column and task command headings. FORM has provisions for only 
two arms. 
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1.3.22 CYL 

Subroutine CYL is called within the system definition function during 
detailed graphic representation generation for the robotic system con- 
stituents (environment, arms, targets, loads). If the requested com- 
ponent is a cylinder or cone, it is called to compute data points for 
the graphic routines. The controlling argument in the call, ISHAPE, 
determines which geometric shape has been chosen in calling routine 
BLDENV, BLDDAT, BLDTRG or BLDENV. The detailed graphic component di- 
mensions are written to a print/save file for archiving the program 
interaction. 
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1.3.23 RECT ‘ t 

Subroutine RECT is called within the system definition function during 
generation of detailed graphic representations for environment, arm, 
target, or load objects file. It is called if the requested component 
is a rectangular solid (ISHAPE ■ 3), a symmetric trapezoidal solid 
(ISHAPE *4), or a nonsymmetric trapezoidal solid (ISHAPE * 5) to com- 
pute data points for the graphic routines. The detailed graphic com- 
ponent dimensions are written to a print/save file for archiving the 
program interaction. 
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1.3.24 TRISTR 

Subroutine TRISTR is called within the system definition function 
during detailed graphics representation generation for the environment, 
manipulator, target, or load objects. If the requested component is a 
triangular cross-section beam (ISHAPE =6), it is called to compute 
data points for the graphics routine. The detailed graphic component 
dimensions are written to a print/save file for archiving the program 
interaction. 
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1.3.25 DATATAB 

Subroutine DATATAB is called within the system definition function 
during detailed graphics representation generation for the environment, 
the robotic arm, target, or load. If the requested component is a data 
tablet structure, it is called to compute data points for the graphic 
routines when the input ISHAPE flag = 8. The detailed graphic com- 
ponent dimensions are written to a print/save file for archiving the 
program interaction. 
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ELEMENT ICOUNT. WITH X. Y, Z FROM RRRRY ELEMENT Ih-OFFSET 
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1.3.26 FILLET 

Subroutine FILLET is called within the system definition function 
during detailed graphics representation generation for the environment, 
the robotic arm, target, or load. If the requested component is a fil- 
let part, it is called to compute the data points for the graphic rou- 
tines. For a concave fillet, the input ISHAPE flag is 9. The detailed 
graphic component dimensions are written to a print/save file for ar- 
chiving the program interaction. 


t 
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1.3.27 OBSTCL 

Subroutine OBSTCL is called within the system definition function 
during graphics representation generation with option ISHAPE equal 10 
for the environment, detailed robotic arm, target, or load. It is 
called if a requested component is an obstacle entity (a choice option 
from BLDENV) or nonplanar structure (for BLDLOD, BLDTRG or BLDDAT). It 
computes data points for the graphic routines. The detailed graphic 
component dimensions are written to a print/save file for archiving the 
program interaction. 


B-78 



Rev A, October 1985 

ORIGINAL PAGE IS 
OF POOR QUALITY 



B-78a 



1.3.28 ORIENT 


ORIENT is called from most of the build options in INITDRVR, allowing 
the user to reposition components. The user can input a rotation se- 
quence consisting of rotation axes and angles, and a translation vector 
to position the origin of the component within the reference coordinate 
system. MAT is called to compute the total rotation transformation ma- 
trix, and MATVEC to transform vectors from the new coordinate system to 
the reference system. The translation vector is then added to each set 
of coordinates. The translation and orientation of detailed graphic 
components are written to a print/save file for archiving the program 
interaction. 
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1.3.29 MAT 

Subroutine MAT is called during the system definition function to com- 
pute the. total rotation transformation matrix defined by the input ro- 
tation sequence and angles. MAT is called from subroutine ORIENT. The 
rotation sequence passed to it determines the transformation matrix 
from the component system to the reference system it calculates. The 
transpose (inverse) of the normal X,Y and Z-axis rotation matrices are 
used. For each rotation in the input sequence, the axis rotation matrix 
is loaded and premultiplied with the current total transformation 
matrix. 



SUBROUTINE MRT 


I - O 


X 


PMS _ utw xnput wwtion awe to orient l-Ink xn curwsnt coowe, era, 
gERQ RMHT INTERMEDISTE ROTRTXON MATRIX 


ROTRTXON AXIS XS X-AXIS 



ROTATION RXIS IS 


RMRT Cl. 1> — COS CBNSJ 


RMRT Cl. 31 - SXN WNP 


•RXIS 


ROTATION AXIS IS Z-AXIS 


RMRT Cl. 1) — COS JANG) __ 


RMRT Cl. 2) — -SINJANG) 


RMRT C2. 1 3 — SIN CRNG3 


RMRT C2. 2) = COS CANG) 


RMRT C3. 3) — 


CONCATENATE TRANS MATRIX WITH RMRT AND LORO INTO TRANS 


DO UNTIL. I . EQ. 3 
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1.3.30 MATVEC 


Subroutine MATVEC is called during the system definition function to 
provide matrix/vector multiplication. The routine is called from 
ORIENT. The vector A is multiplied by the matrix TRANS to produce out- 
put vector B. Note that this matrix/vector multiplication is 3-D only. 


SUBROUTINE MRTVEC 



H 

II 

0 

1 

i 


X = I 1 

- 1 


L 

II 

0 


B CI3 - PRODUCT OF MATRIX TRANS. 
EL.MT. Cl. U 3 VECTOR R. ELMT. CU5 

DO UNTIL U . EQ. 3 

DO 

UNTIL X . EQ. 3 

RETURN 

END 


/' 
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.3.31 DRAW 

Subroutine DRAW is called within the system definition function to pro- 
vide the graphics display during the generation of arm detailed repre- 
sentations. It is called to display each successive component as it is 
defined. The routine logic is controlled by flag inputs specifying 
initialization (at which time base/ link/ tool transformation matrix con- 
catenations to the system are performed), component drawing, or com- 
ponent modification world. 
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ORIGINAL PAGE IS 
1.3.32 ESMAT Op POOR QUALITY 

Subroutine ESMAT uses Evens end Sutherlend grephics routines to con- 
struct the required trens forms t ion matrices from eech system section 
coordinete system to the grephics coordinste system. Input argument K 
specifies which system section is under consideration. It is celled 
during execution of the system definition function. It is called from 
subroutine DRAM to compute the required transformation matrices for each 
system section. The robotic system has section coordinate systems for 
the base, each joint/link, and the end-effector. An input value of K*1 
indicates the robotic system base. The transformation matrix is com- 
posed of a translation matrix based on the base location and rotation 
matrices constructed using the base orientation parameters. A value of 
K from 2 to the number of links plus 1 (NJ+1) indicates the (K-l)th 
joint/link. All transformation matrices from each of the sections to 
the previous joint (or base, if the current joint is the first joint) 
are concatenated to form the total transformation matrix to the graphics 
coordinate system. Each joint transformation matrix is composed of a 
translation matrix based on the joint position, a rotation matrix based 
on the initial joint angular displacement, and rotation matrices for 
joint orientation. A value of K»NJ+2 indicates the end-effector system. 
The transformation matrix for the end-effector is composed of a trans- 
lation matrix, and rotation matrices for end-effector orientation. The 
end-effector location and orientation are specified' relative to the 
coordinate system of the final joint in the system. 


SUBROUTINE ESMRT 


TV 

PPM BPSE 


9CflLC ORB g UOCRTZON BY 

XFRCT RNO L-ORD INTO INTEGER RRRRY 


i CRUU TTRRN FOR PICTURE PROCESSOR TRRNSPORMRTXON MRTRIX 

* — ; 

gxmncT RowioN rxw used 

2N ORXENTZNG. FROM UOXNT SEQUENCE fWWVTV 

i 

extfrct x*y. s rot. angles 

USKO IN ORIENTING* FROM UT. ANGLE RRRRY 

— ^ 

C«UL TROTX. -Y OR -E WITH 

INTEGER PNG* TO ROTRTE TRR 43FOHHH 1 XUH 


CALL. TSET TO LORD 1 W-LH FA 

RAY WITH CURRENT PICTURE PROCBJIUOM TFRNS. 


K ONE OF THE RRM JOINTS 


|SCRUE JT. LOCATION BY 

XFRCT RNO LORO INTO INTEGER RRRRY j 


I CRUU TTRRN FOR PICTURE PROCESSOR TRRNSFORMRTION MRTRIX ! 


EXTRACT OFF-SET JT. 

RNG. FROM UOXNT VRRIRBUE RRRRY 

. — . 

CRUU TROTX OR — Y WITH 

JT. RNG. TO ROTRTE TRRNSFORMRTION 

— i 

EXTRRCT X. Y. Z ROT* AWSE-Ea 

USED IN OPaCNTINO, FROM UT. PNOLE RRRRY 

[CALL. TROTX. -Y OR WITH INTEGER RNG. TO ROTRTg TRm l^FCini nTION \ 


nm i t9«t to loro imnxji m 

RRRY WITH CURRENT FZCTURE PAOCEigm TRRNGL 




RRM TOOL. 


SCRUB TOOL LOCATION 

BY XFRCT RNO UORO INTO INTEGER 
RRRRY 


CRUU TTRRN FOR RXCTURB 

E PROCESSOR TRRNSFORMRTION MRTRIX 


j EXTRRCT ROTRTION AXES USKO ZN ORIENTINQ. FROM JOINT 

* ; 


SEQUENCE RRRRY 

i 

[EXTRRCT X. Y. 2 ROT. PN 

SUES USED IN ORIENTING. FROM JT. 
PNQUC RRRRY 


CRUU TROTX. — Y OR — 

E WITH INTEGER RNG. TO ROTRTE 
TRRNSFORMRTION 


CRUU TGET TO LORD MRTRIX RRRRY WITH CURRENT PICTURE 

PROCESSOR TRRNS- 


RETURN 


1 END 
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1.3.33 DBAS 

Subroutine DBAS is called within the system definition function during 
detailed graphics representation generation. For the subroutine, input 
calling argument IMAN specifies environment, robotic system component, 
target, or load objects file consideration. Graphics object data 
IOBJBK are loaded for robotic system components, TGRAFBK is loaded for 
target components, LGRAFBK is loaded for load components, and ENVTBK is 
loaded for environment components. The manner in which the data are 
stored in the COMMON blocks is dictated by the data format used in 
Evans and Sutherland graphics routine D3DATA. 
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1.3.34 BASELK 

In the create mode of BASELK, the user is prompted for the base mass, 
the location of the base center of mass in the base coordinate system, 
the base inertia matrix relative to the centroid, the point mass values 
and locations if desired. 
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SUBROUTINE BflSELK 
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1.3.35 RCTSTR 

Subroutine RCTSTR is called within the system definition function 
during detailed graphics representation generation for the environment, 
manipulator, load, or target objects. If the requested component is a 
rectangular cross-section beam (ISHAPE=7), it is called to compute data 
points for the graphics routine. 
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1.3.36 CADOBJ 

Subroutine CADOBJ is called within the system definition function 
during detailed graphics representation generation for the environment, 
manipulator, load, or target objects. If the requested component is a 
CAD/CAM object (ISHAPE=11), it is called to compute data points for the 
graphics routine. 
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1.3.37 TARG 

Subroutine TARG is called within the system definition function during 
detailed graphics representation generation for target objects. If the 
requested component is a 4-dot target object (ISHAPE=12), it is called 
to compute data points for the graphics routine. 
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1.4.2 MATMPY 


Subroutine MATMPY performs the multiplication of two matrices, AB - C, 
where A has I rows and J columns, the dimension of B is JxK and C is 
IxK. The matrices and their sizes are passed to subroutine as calling 
arguments. 


SUBROUTINE MRTMPY 



END 



1.4.3 ERBMSG 


Subroutine ERRMSG is called when certain errors occur during R08SIM 
execution. The routine first displays the current operating mode 
(i.e., system definition, analysis, or postprocessing). The routine 
searches the file ERROR. OAT for an error message corresponding to the 
error number passed to it. The message is typed at the terminal and 
execution returns to the calling routine, from which it continues or 
terminates depending on whether the error is fatal. 


SUBROUTINE ERRMSG 


DISPLAY WHICH SECTION OF ROBSIM 
ERROR OCCURRED IN 


OPEN ERROR MESSRGE FILE 


SERRCH FILE FOR APPROPRIATE MESSRGE 


MESSRGE FOUND 


WRITE ERROR 
MESSRGE 


DISPLAY STRTEMENT 
THRT RPPROPRIRTE 
MESSRGE NOT FOUND 


CLOSE ERROR MESSRGE FILE 















1.4.4 ROTMAT 


ROTMAT computes a rotation matrix from a sequence of up to three rota- 
tions about coordinate axes. It decomposes the input calling argument 
JSEQ into three successive rotation axes, computes each corresponding 
rotation matrix from the specified angles of rotation and combines these 
successively to find the overall rotation matrix. 


SUBROUTINE ROTMAT 


INITIALIZE ARRAY T2 C3. 33 TO AN 
IDENTITY MATRIX 


DETERMINE INDIVIDUAL ROTRTION RXES 

CIUO C33 3 


IJO CI3 - NE. O 

Cfll_l_ CETM TO FIND THE 
TRANSFORMATION MATRIX 
T 1 (3. 33 FOR A SINGLE POTATION 

CNULL3 

CAUL. MATMPY TO COMBINE 
NEW MATRIX T1 WITH OUD 
MATRIX T2 


DO UNTIL. 1=3 


PUT RESULT IN ARRAY RM C3. 33 


RETURN 


END 
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1.4.5 CETM 


Subroutine CETM calculates a transformation matrix for a specific input 
axis of rotation and rotation angle by the use of appropriate direction 
cosines matrix. The calling argument input is: 

Symbol Type Dim. Definition 

IAXIS 1*4 1 Rotation axis for joint orientation 

* 1, Rotation about x-axis 
m 2, Rotation about y-axis 
» 3, Rotation about z-axis 

TH R*4 1 Rotation angle for joint orientation 


SUBROUTINE CETM 



(NULL) 



1.4.6 LOGO 

The LOGO routine calculates data points required to output the Martin 
Marietta logo, and displays it on the robotic simulation E&S graphics 
display. It extracts from the data points file LOGO.DAT, the Martin 
Marietta company logo, scales and displays the logo for the robotic 
graphics simulation. 


SUBROUTINE LOGO 



OPEN 

LOGO DRTR FILE 


READ OF LOGO DHTfl INTO INTEGER DRTR RRRRV 


CLOSE 

LOGO DRTR FILE 

i 

‘SET INTEGER STEP 

VALUE TO SO/INPUT SIZE 

i 

DO 

FOR ERCH DRTR HORIZONTRL PIXEL 

■ ■ i 


L. = O ..... . 


INCREMENT I 

' 

fXNTESeH X# Y VffLUga — flWWY CL-MT . *INFUT aXFPn ♦ INXTIflL X* Y 


INTEGER Z VALUE = INITIAL Z 


L = L ■+• 1 i 


|SET OUTPUT LOGO RRRRY 

X. Y. Z. EL.EMENT U. TO X. Y. Z 

_ 

i 

iSET START RND STOP 

POINTS LOOP PRRRMETERS 


i 

1 U » O 


Ido 

WHILE l_. NUMBER OF POINTS IN LOGO. . L.E. 500 



i — 

U — U -t- 1 



i 

i 

DETERMINE RH TIO TO USE IN OU fPUTTXNS LOGO POINTS 


i 

i 

REWRITE INTEGER X 

VRLUE - INTEGER X ~ RATIO 



i 

joo u 








f 

| ”■ RETURN i 

nz 


END 

- — - 
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1.4.7 CRPD 


Subroutine CRPD computes the cross-product of two vectors A and B, each 
containing three components. The result is put into the vector C. 
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The Analysis Tools Function 


The program SIMDRVR is the analysis tools function driver. The following 
set of routine functional descriptions and VCLRs (visual control logic repre- 
sentations) are the modules found in the analysis tools function of ROBSIM. 


2.0 

I SIMDRVR 



RDSYS 


ERRSMG 


2 . 1.5 


TASK 


REQUIR 


2 . 2.241 


SEGTASK 


2 . 1.2 


SEGMNT 


1 . 2.11 


CVTUNIT I SETUP 



- CETM 

- MATMPY 


FTIN 


- ERRMSG 


2.121 
SETUP 2 


2.1.6 I 


— — 1 . 4.11 1 . 4 . 4 | 



1 


I 



BASPUT 

MATMPY 

CETM 

1 

JACOB | 









- CRPD 


CVTUNIT I ROTMAT 


1 . 4.3 


ERRMSG 


*Used often, so referred to by * in subsequent flowcharts. 


Figure B-8. - Functional block diagram for SIMDRVR. 


PRECEDING PAGE BLANK NOT Ruw£D 

























ORIGINAL PAGE IS 
OE POOR QUALITY 



REQUIR 


Rev A, October 1985 


2 . 2.2 


2.2.3 


1 . 2.11 


REQOPT 


PRTARM 


: . 1.21 

LBR_HELP 

2 .3.441 
BASINIT 
2.4.6 1 
I BORERR 


1 . 4.3 


SETUP 


2.2.25 


2.2.4 1 
GRAFIX 


2.2.5 


2 . 2.6 


2 . 2.8 


CNTRLR 


SPRGINC 


E&S ROUTINES 


ERRMSG 


2.3.451 


BRCNTRL 


2.3.71 
I RATEPRO 


BCNTRLR 


2.3.46 


1.4.3 


BPCNTRL 


ERRMSG 


2 . 3.1 


1 . 4.1 


2 . 3.41 


2 . 3.5 


2 . 3.61 







CHKLMT 


DYNAM 


2 . 2 . 9 | 

I VOLTAGE 


2.2.10 


2.2.11 


2.2.12 


OUTREQ 


ESPAUS 


ENDREQ 


(Not yet 
implemented) 


- E&S ROUTINES 


2 . 3.13 


2 . 3.14 


2 . 3.15 


2 . 3.16 


REQPRT 


REQSOF 


REQTRQ 


REQPLT 


2 . 3.8 


1 . 2.11 


PCNTRL 


ERRMSG 


SETUP 


2.3.17 


2.3.47 


2 . 3.9 


- CRPD 


2 , 3.10 


!. 3.11 


2 . 3.12 


2 . 3.3 


POSGRDJT 


TRACKING 


GRASP 


RELEAS 


- MATMPY - MATMPY 


- MATMPY 

- CRPD 

- ERRMSG 


- CRPD 


1 . 2.11 


1 . 3.19 


JACOB 


2 . 4.11 

SLVLIN2 


12.3.481 1.4T2I 


FORCE 


2 . 3.19 


TORQUE 


1 . 4.7 


ACTORQ 


1 . 4.2 


2 .3 .511 


12. 3.521 


INITTAR 


2.3.49 


MATMPY 


PERSPECT HARALICKR 


| CVTIN 
- MATMPY 


MATMPY 


ADDMAS 


ADDMAS2 


RCICR 


SETUP2 - MATMPY 2 4 2 

T— — 1 - CRPD f — ' 


ANGLES 


2 . 3.20 


2 . 3.21 


2 . 3.22 


REP COL 


2.3.501 

NEWFRAME 


SPRGFOR CNSTFOR 


PTACC 


1 . 2.11 


1 . 4.2 


2.3.18 


1 . 3.19 


2.4.1 


2 . 4.3 


- CRPD 

- MATMPY 

- CRPD 


SETUP2 


MATMPY 


JTPOS 


JACOB 


SLVLIN2 


- MATMPY 

- CRPD 2 - 4 - 2 


ORERR 
- CRPD 


REPCOL 


2.4.3 | 
ORERR 
- CRPD 


1 . 3 . 19 | 

| JACOB 

- MATMPY 

- CRPD 


2 . 4.1 


1 . 2.11 


SLVLIN2 


SETUP2 


2 . 4.2 


REPCOL 


*Defined in previous flowchart |SIMDRVR~ 


Figure B-8. - (cont) 
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*Defined in flowchart 


SIMDRVR . 


**These subroutines defined in previous flowchart under 


REQUIRl . 


^_FGL00UT FRAA1E 

Figure B— 8. - (concl) 
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TABLE B-VI . - PROGRAMS EMPLOYED IN SIMDRVR 


2.0 

SIMDRVR 

2.3.7 

2.3.8 

2.1.1 

RDSYS 

2.3.9 

2.1.2 

SGMNT 

2.3.10 

2.1.3 

REQUIR 

2.3.11 

2.1.4 

RESPON 

2.3.12 

2.1.5 

TASK 

2.3.13 

2.1.6 

BASGMNT 

2.3.14 

2.3.15 

2.2.1 

FTIN 

2.3.16 

2.2.2 

REQOPT 

2.3.17 

2.2.3 

PRTARM 

2.3.18 

2.2.4 

GRAFIX 

2.3.19 

2.2.5 

CNTRLR 

2.3.20 

2.2.6 

SPRGINC 

2.3.21 

2.2.7 

CHKLMT 

2.3.22 

2.2.8 

DYNAM 

2.3.23 

2.2.9 

VOLTAGE 

2.3.24 

2.2.10 

OUTREQ 

2.3.25 

2.2.11 

ESPAUS 

2.3.26 

2.2.12 

ENDREQ 

2.3.27 

2.2.13 

SIMOPT 

2.3.28 

2.2.14 

INITCO 

2.3.29 

2.2.15 

DEFCNST 

2.3.30 

2.2.16 

PIDINIT 

2.3.31 

2.2.17 

OUTSIM 

2.3.32 

2.2.18 

CNTRSIG 

2.3.33 

2.2.19 

CONTROL 

2.3.34 

2.2.20 

SETCNST 

2.3.35 

2.2.21 

DERIV 

2.3.36 

2.2.22 

INTGRT 

2.3.37 

2.2.23 

ENDS IN 

2.3.38 

2.2.24 

SEGTASK 

2.3.39 

2.2.25 

B CNTRLR 

2.3.40 

2.3.41 

2.3.1 

SEGINIT 

2.3.42 

2.3.2 

GRASP 

2.3.43 

2.3.3 

RELEAS 

2.3.44 

2.3.4 

ESCNTRL 

2.3.45 

2.3.5 

POSSPJT 

2.3.46 

2.3.6 

RCNTRL 

2.3.47 


RATEPRO 2.3.48 INITTAR 

PCNTRL 2.3.49 ANGLES 

CABSIM 2.3.50 NEWFRAME 

FORCE 2.3.51 PERSPECT 

TORQUE 2.3.52 HARALICKR 

ACTORQ 2.3.53 BDRTORQ 

REQPRT 2.3.54 SLVMBAS 

REQSOF 

REQTRQ 2.4.1 SLVLIN2 

REQPLT 2.4.2 REPCOL 

POSGRDJT 2.4.3 ORERR 

JTPOS 2.4.4 OUTUN 

CVTIN 2.4.5 ICVTATD 

SPRGFOR 2.4.6 BORERR 

CNSTFOR 

PTACC 

POSSENS 

SIMPRT 

SIMPLT 

FORTOR 

FORREF 

CMPCTRL 

PIDCON 

PIDFOR 

EFINRT2 

NLINK 

SIMLMT 

STOPFR 

ACTIVPIH 

DRTORQ 

EFINRT 

SLVTHDD 

LININT 

LDVOLT 

CALCI 

SOLVE 

GAUSS 

BASINIT 

BRCNTRL 

BPCNTRL 

TRACKING 
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2.0 SIMDRVR 

The program SIMDRVR is the analysis tools function driver. It operates 
in an interactive mode, prompting the user for the analysis option de- 
sired: requirements analysis without graphics, requirements analysis 

with graphics (a display of system motion during program execution), re- 
sponse simulation analysis without graphics, response simulation analysis 
with graphics, option to set up a base or arm motion program or terminate 
SIMDRVR execution. 
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2.1.1 RDSYS 

Subroutine RDSYS is called from SIMDRVR to read in the manipulator sys- 
tem definition data needed to run any of the SIMDRVR analysis options. 
The routine first prompts the user for the name of the file containing 
the system's data and then opens that file. If the system includes 
moving bases it reads the number of bases. Moving base numbers, geo- 
metric properties, mass properties, actuator properties and special 
joint data for each arm are read in, as well as system graphics data 
anri the definition of gravity for the system. If the system contains 
an environment, the data describing it are read in. If load objects 
are also to be included, the data describing them are read in. End- 
effector data and target data are to be read next. After that, the 
file is closed and saved. 
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2.1.2 SGMNT 

Subroutine SGMNT allows the user to set up the desired motion profile 
for a requirements analysis or response simulation run. It is called 
from SIMDRVR. An existing motion profile file may be read in and modi- 
fied or the profile may be defined interactively. Motion is specified 
in one of four ways: 

1) Desired position of end-effector; 

2) Desired position of each joint; 

3) Rate of end-effector movement; 

4) Rate of each joint. 

In addition, motion may be specified by having an end-effector-mounted 
sensor move toward a target. Several nonmot ion— type operations such as 
grasp a load object, release object and wait a given length of time may 
also be specified. 
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2.1.3 REQUIR 

Subroutine REQUIR is called from SIMDRVR and is the routine that con- 
trols the execution of any requirements analysis run. It first calls 
REQOPT to set up program run time options. If requested, PRTARM is 
called to write a description of the system to an output file. SETUP 
is called to calculate initial positions. GRAFIX is called if the run 
is to include graphic displays. The subroutines CNTRLR, SPRGINC, 

CHKLMT, DYNAM and OUTREQ are called at every increment of a user-defined 
time loop to calculate the manipulator system's motion, forces and 
torques, and write these data to an output file. ESPAUS is called when 
motion is temporarily halted during execution. When the stop time is 
reached, ENDREQ is called to close any open files. 
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2.1.4 RESPON 

Subroutine RESPON is called from SIMDRVR to control the execution of a 
response simulation run. Run time options and program variables are 
first initialized. A user-defined time loop is executed to call rou- 
tines to carry out all the control functions. After execution is com- 
pleted, ENDSIM is called to close the files. 
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2.1.5 TASK 

Subroutine TASK is the preliminary routine called when defining manipu- 
lator motion. The user has the choice of modifying an existing time 
history (motion) profile, creating a new profile or writing a user 
readable, formatted file from an existing time history profile. The 
subroutine opens the appropriate files and then calls subroutine SGMNT 
if an existing file is being modified, a formatted file is to be writ- 
ten, or a new file is to be created using just lower level motion com- 
mands. If task level commands are used, TASK calls subroutine 
SEGTASK. For other options task calls SGMNT. 
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2.1.6 BASGMNT 

Subroutine BASGMNT allows the user to set up the desired base motion 
profile for a requirements analysis or response simulation run. It is 
called from SIMDRVR. An existing base motion profile file may be read 
in and modified or the profile may be defined interactively. Base mo- 
tion is specified in one of two ways: 

1) Desired position and orientation of the base; 

2) Rate of base motion. 
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2.2.1 FTIN 


Subroutine FTIN is called from SGMNT if force/torque or active compli- 
ance control was specified by the user. If force/torque control was 
specified, the user is prompted for the number of force and torque com- 
ponents to be controlled, the unit vectors in the directions to be con- 
trolled and the magnitude of the control force or torque. If active 
compliance control was specified, the user is prompted for the stiffness 
matrix at the end-effector reference point. 


SUBROUTINE FTIN 



IOP . EQ. 5 



PROMPT USER FOR FORCE/TORQUE CONTROL. ON/OFF 

FLAG 



TURN FORCE/TORQUE CONTROL ON 



PROMPT USER FOR NUMBER OF FORCE 
COMPONENTS TO CONTROL. 



■ 

PROMPT USER FOR CONTROL DIRECTION 




PROMPT USER FOR MRGNXTUOE OF CONTROL FORCE 



DO 

FQF 

UNTIL. N . EQ. NUMBER OF CONTROLLED 
qCE COMPONENTS 


g 

PROMPT USER FOR NUMBER OF TORQUE 
| COMPONENTS TO CONTROL. I 

g 


■ 

PROMPT USER FOR CONTROL DIRECTION 



1 

PROMPT USER FOR MRGNXTUOE OF CONTROL 

TORQUE 



DO UNTIL. N . EQ. NUMBER OF CONTROLLED 
TORQUE COMPONENTS 





x IOP . EQ. S 

st 

PROMPT USSR TOR COMPUXRNCS CONTROL. OKU'OFR TORO ~ "1 


TURN COMPLIANCE CONTROL ON /£ 

CNULL3 

OF CNULL3 


RETURN 

END 


B-102 











Rev A, October 1985 


2.2.2 REQOPT 

Subroutine REQOPT is called from REQUIR to define requirements analysis 
run time options. The user may list currently defined options and use 
them or input a new set of options. Options the user may set include 
run time data file write, torque file write, control method to be used, 
control of robot base, dynamic calculations, playback file write, plot 
file write, and simulation start time, stop time, and processing step 
size. 
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2.2.3 PRTARM 

Subroutine PRTARM Is called form either REQUIR or RESPON when the flag 
for printed output of that analysis is set. This routine prints a de- 
scription of the manipulator system that includes the following vari- 
ables: current arm number and number of joints per arm, type and mass 

of each joint, initial angular positions and velocities of each joint, 
joint travel and rate limits, joint/link centroid locations, joint lo- 
cation relative to previous joint, inertia matrix for each joint, orien- 
tation matrix for each joint relative to previous joint, span of the 
whole system, and the acceleration attributable to gravity. 


SUBROUTINE PRTRRM 



WRITE CURRENT RRM NUMBER RND THE 
NUMBER OF JOINTS PER RRM 

WRITE THE TYPE RND MRSS OF ERCH JOINT 

CONVERT DRTR TO BE WRITTEN FROM 
INTERNAL TO INPUT/OUTPUT UNITS 

WRITE JOINT INITIAL ANGULAR POSITIONS 

RND VELOCITIES 

WRITE JOINT TRRVEL RND RATE LIMITS 

WRITE JOINT/LINK CENTROID LOCRTIONS 

WRITE JOINT LOCATIONS RELATIVE TO 

PREVIOUS JOINT 

WRITE INERTIA MATRICES FOR JOINT/LINK 

COMBINATIONS 

WRITE ORIENTATION MATRICES FOR EACH 
JOINT RELATIVE TO PREVIOUS JOINT 

DO 

UNTIL. KARM - NUMBER OF ARMS IN THE SYSTEM 

WRITE TOTRL SYSTEM SPRN 

WRITE RCCELERRTION DUE TO GRRVITY 

RETURN 

END 


i 


I 
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2.2.4 GRAFIX 

Subroutine GRAFIX provides the motion graphics capability in the re- 
sponse simulation, requirements analysis and postprocessing func- 
tions. GRAFIX displays the environment, target, load and robotic sys- 
tem motion within the environment. If IFLAG=1, the graphics system is 
initialized and displayed in the initial condition; if IFLAG=2, the 
display is updated to the current time step condition; if IFLAG=3, the 
motion is complete and the graphics are terminated. 
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2.2.5 CNTRLR 

Subroutine CNTRLR is called from REQUIR to obtain the angular position, 
velocity, and acceleration for each joint of each arm at each processing 
time step. If the variable IDATA was set to 1 earlier, the data are ob- 
tained by reading an existing file that contains just these data. If 
IDATA equals 2, the values are calculated from the motion profiles. 
Subroutine PCNTRL is called for the position control calculations and 
RCNTRL is called for the rate control calculations and TRACKING is 
called for sensor control. IDATA equal to 3 allows the system motion 
to be controlled by dials on the Evans and Sutherland. 
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2.2.6 SPRGINC 


Subroutine SPRGINC is called from REQUIR to set the variables used when 
the end-effector is to have compliance associated with it. The vari- 
ables set includes spring reference position, orientation and the spring 
constant. 
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2.2.7 CHKLMT 


CHKLMT checks joint displacement and rate limits during requirements 
analysis. It does not modify any values but prints a warning to the 
terminal if any limits are exceeded. 


SUBROUTINE CHKLMT 


DO FOR EACH RRM IN SYSTEM 


DO FOR EACH JOINT IN RRM 


Nv DISPLACEMENT EXCEEDS y/ 

T X MINIMUM OR MRXIMUM VALUE y'p 

TYPE WARNING TO 
TERMINAL WITH 
PERTINENT DRTR 

(NULL) 

\ RRTE EXCEEDS MINIMUM OR / 

T X MRXIMUM VALUE x' F 

TYPE WRRNING TO 
TERMINRL WITH 
PERTINENT DRTR 

CNULL) 



JOINT RRTE EQUATIONS COULD 
NOT BE SOLVED 


TYRE UIRRNING TO 
TERMINRL WITH PERTINENT 
DRTR 


CNULL) 
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2.2.8 DYNAM 


Subroutine DYNAM is called from REQUIR to compute the manipulator sys- 
tem dynamics at each processing time step by calling the SETUP, CABSM, 
FORCE, TORQUE, and ACTORQ subroutines. 


SUBROUTINE DYNRM 

CRLL SETUP TO FIND RLL POSITIONS 
IN WORLD COORDINRTES 


CALL CRBSM TO FIND RBSOLUTE VEI — 
RND RCCEL. OF RLL LINKS 


CRLL FORCE TO FIND JOINT RERCTION 

FORCES 


CRLL TORQUE TO FIND JOINT RERCTION 

TORQUES 


CRLL RCTORQ TO FIND JOINT RCTURTOR 

TORQUES 


RETURN 


END 
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2.2.9 VOLTAGE 


(Not implemented yet.) 
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2.2.10 OUTREQ 

Subroutine OUTREQ is called from REQUIR to write output data to files 
requested by the user. The files the user may elect to have data writ- 
ten to are: 

1) Run time output data file; 

2) Data file for subsequent replay of motion on a vector graphics sys- 
tem; 

3) Actuator torque data file; 

4) Run time data file for subsequent plotting; 

5) Base torques and forces data file. 
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2.2*11 tsSi-AUS 


Routine ESPAUS is responsible for polling the status of the E&S function 
keys to determine the on/off status of the devices switch for playback 
motion cessation. A light indicator in the function key is used to in- 
form the user of the key status; when lighted, the perspective viewing 
is in operation. 



SUBROUTINE 

ESPRUS 


STATUS FUNCTION KEYS CEXTENDED 
SWITCHES) EVENT QUEUE 

T 

USER FUNCTION KEY FOR 
TERMINATING IS RCTIVRTED 


ISTOP OUTPUT 
FLAG - O 



(NULL) 

j\ 

T 

USER FUNCTION KEY FOR 
. CESSRTION OF MOTION IS 
\ RCTIVRTED 


ISTOP OUTPUT 
FLAG - 1 

CNUI_I_) 

RETURN 

END 
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2.2.12 ENDREQ 


Subroutine ENDREQ closes any files opened during running of the require 
ments analysis portion of ROBSIM. 


SUBROUTINE ENDREQ 



FILE 3 IS 

OPEN yp 

CLOSE LU3 

CNULL3 


FILE S IS 

OPEN /P 

CLOSE LU6 

CNULL3 | 

■■ ■ ■ rA 


FILE 13 IS 

OPEN /P 

CLOSE LUIS 

(NULL) 


FILE 1 <4 IS 

OPEN / w 


CLOSE LU14 

r 



CNULLJ 


FILE 1 6 IS 

OPEN /p 

CLOSE LU16 

(NULL) 

1 

FILE IT CKRRM3 

IS OPEN 

■ 

CLOSE LU1T CKRRM3 

(NULL) 

DC 

UNTIL KRRM . EQ. NUMBER OF RRMS 

' RETURN 

~~ END 
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2.2.13 SIMOPT 

Subroutine SIMOPT interactively prompts the user for the program start 
time, stop time, processing time step, and several flags for control of 
output and the selection of some computational capabilities. Among 
these output options is a simulation output file that contains the data 
required by the postprocessing function for further study. The user 
also specifies the time frequency of the output of data to the file. 

The user is also allowed to request printed output during the analysis 
tools function execution. 

The content and format of the data to be printed are provided for within 
each of the analysis tools. The flag set within SIMOPT is used only to 
turn the print routines on. The time frequency of the printed output 
is also specified. Other options are for generation of an acceleration- 
velocity-theta file and/or a plot output data file that may be plotted 
with the ROBSIM postprocessing plot utility with their associated out- 
put time steps. The user may also request use of a torque input file 
or a control option to read a hardware input voltage file for computa- 
tional capabilities. 
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SUBROUTINE SIMOPT 


ORIGINAL PASS S3 

OF POOR QUALITY 
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2.2.14 INITCO 

Subroutine INITCO prompts the user for the initial joint position (TH) 
and velocity (THD) of each joint of each arm. If moving base is simu- 
lated, the routine prompts the user for the initial base positions, 
orientations and velocities. 
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2.2.15 DfiJrCNSx 

DEFCNST reads a file containing the information needed to define a con- 
straint (either planar or peg-in-hole type) on the end-effector motion 
during dynamic simulation of the arm response. The user specifies the 
name of the constraint file in response to interactive prompts. 


SUBROUTINE DEFCNST 

QUERY WHETHER USER WANTS TO INCLUOE CONSTRAINT 


^ CONSTRRINT DESIRED 

— — ■ — - " " " * . 1 * ' '* " *' ' \ tf-i 


KRRM = 1 

CO 
1 — 

PROMPT FOR FILENAME OF CONSTRRINT FILE 

1 

CL 

OPEN CONSTRRINT FILE 

CO 

READ TOOL REFERENCE POINT LOCRTION 

C-J 

RERD NUMBER OF PLRNRR CONSTRAINTS 


□O FOR EACH PLRNRR CONSTRRINT 


UJ 

RERO THE 4 COORDINRTES DEFINING THE PLANE 


CE 

RERD NUMBER OF PEG-IN— WOLE CONSTRAINTS 

hH 

CD 

DO FOR ERCH PEG— IN-HOLE CONSTRRINT 


H-H 

RERD HOLE LOCRTION 

■ 

H— 

RERD DIRECTION OF HOLE AXIS 

■ 

CO 

LlJ 

RERD DEPTH OF HOLE 

■ 

OD 

RERD RADIUS OF HOLE 

■ 

HH 

cn 

RERO FRICTION COEFFICIENT FOR HOLE 

■ 


CLOSE CONSTRRINT FILE 

LU 

CO 


HOLE CONSTRRINT INCLUDED 

2 

(ACTIVATE HOLE CONSTRAINT 

OerXNC HOLE CONSTRRINT INRCTIVE 



















2.2.16 PIDINIT 


Subroutine PIDINT is called from RESFON to initialize variables used in 
the program's control algorithms. POSSENS is called first to determine 
the actual joint positions. Initial values for some control variables 
are set. The user is then asked to supply system gains for the methods 
of control that will be used during program execution. These gains may 
be supplied by either reading in a file of existing gains or by the user 
interactively Inputting the gains. 


SUBROUTINE PIDINIT 



i 

I 

i 

I 

| 


1 

i 


L 
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2.2.17 OUTSIM 


Subroutine OUTSIM is called from RESPON to write the appropriate output 
data to the different types of files requested by the user. Types of 
output files available are: 

1) File of run time data for subsequent tabular printout; 

2) File of joint positions, velocities and accelerations as functions 
of time ; 

3) Data file for later motion replay on vector graphics machine; 

4) File of data for subsequent x-y plotting. 


SUBROUTINE OUTSIM 


3 


FIRST CRI_L_ TO SUBROUTINE 


TIME FLROS TO S TR R T TIME 


CNUL.L.3 


^ CQF 


IPR1NT . L.S. 2 


CORRECT TIME TO WRITE DATA 


CNUL-LJ 




CNUL_l_3 


DO UNTIL. KRRM , EQ. NUMBER OF RRMS 


3 


IDRTR . EQ. 


^ CORRECT TIME TO WRITE DRTR 


WRITE CURRENT TIME TO OUTPUT FILE 

CNUI—l—3 

| WRXTff TUm TWO# THOO TO OUTPUT KILE 

DO UNTIL. KRRM . CEO. NUMBER OF RRMS 


CNUL-1—3 


3 


ISIMO . EQ. 



CORRECT TIME 

TO WRITE 

DRTR Sw\ 

WRITE CURRENT TIME TO 

OUTPUT FILE 



““ 

WRITE TH. U_D TO 

OUTPUT FILE 


CNUI_L_3 

DO 

UNTIL KRRM . EQ. NLrt 

>IBCR OF RRMS 




IPLOT - EQ. 


» ^ — 

CORRECT TIME TO 

WRITE DRTR 

CPt-L. SIMPLT TO WRITE 
PLOT OUTPUT FILE 

CNUL-L.3 


CNUI—l—3 


RETURN 

END 


(NULL) 














2.2.18 CNTRSIG 


Subroutine CNTRSIG is called from the routine REQUIR. Joint variables 
are stored in dummy variables and CNTRLR is called to calculate joint 
angular reference positions and velocities. The end-effector position 
error is calculated and ORERR is called to determine the orientation 
error. If force/torque control is being used, subroutines FORTOR and 
FORREF are called to calculate joint reference positions and reference 
forces and torques. If active compliance control is being used, sub- 
routine CMPCTRL is called to calculate amplifier input voltages. 


SUBROUTINE CNTRSIG 
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2.2.19 CONTROL 


Subroutine CONTROL is called from RESPON at every processing time step. 
If a feedback control law is to be used, POSSENS is called to get the 
actual joint data and PIDCON is called to get actuator voltages for PID 
control. If force/torque control is being used, PIDFOR is also called 
to get actuator voltages caused by the force— controlled components. 


SUBROUTINE CONTROL 




CRLL POSSENS (KRRM) TO GET 
RCTURL UOINT DRTR 

CRLL PIDCON CKRRM) . TO GET 
RCTURTOR VOLTRGES FOR PID 

CONTROL 


\ IFTCN . EQ. 1 / 

T \ / F 

CPU- PIOFOR (KPRMJ TO 
SET RCTURTOR VOLTRGE5 
FOR FORCE/TORQUE CONTROL 

(NULL) 


DO UNTIL KRRM . EQ. NUMBER OF RRMS 


RETURN 


END 
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2.2.20 SE TONS' 


SETCNST checks planar constraints to see if they are violated or need 
to be activated. If the current velocity violates the constraint, the 
velocity impulse to satisfy the constraint is evaluated. 


SUBROUTINE SETCNST 


DO FOR EACH RRM 


\ NUMBER OF PLflNRR CONSTRAINTS 

T\ GRERTER THAN ZERO 


EVRLURTE DISTANCE TO CONSTRAINT 

PLANE 


X DISTANCE LESS THAN ZERO 


PRINT ■CONSTRAINT VIOLATED* 
WARNING TO TERMINAL 


(NULL] 


V DISTANCE LESS THAN TOLERANCE 


SET UP FLAGS TO ACTIVRTE 
CONSTRAINT 


EVALUATE VELOCITY OF POINT 
TOWARD CONSTRAINT PLANE 


V VELOCITY GRERTER THAN ZERO 


EVRLURTE JOINT RRTE IMPULSE 
TO MRKE VELOCITY ZERO 


PRINT ■VELOCITY IMPULSES 
WARNING TO TERMINAL 


UPDATE JOINT VELOCITIES 



(NULL) 
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2.2.21 DERIV 

DERIV is used during response simulation to interface between INTGRT and 
the dynamics module NLINK. This routine puts the state vector Z into 
the appropriate common variables* calls NLINK and puts the results from 
the common variable THDD into ZD. 
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2.22 INTGRT 


OKaGSi'IAL f-A52 13 

OF. POOR QUALITY 


Subroutine INTGRT Is called from RESPON and uses a fourth-order Runge- 
Kutta algorithm to Integrate a state vector Z. State derivatives are 
computed by the subroutine DERIV. 


SUBROUTINE INTGRT 

SET T TO INITIAL TIME FOR INTEGRATION 
SET HPT TO TXHE STEP/2. 


DO FOR ERCH COMPONENT XN STRTE 






CRl_l_ STOPFR TO STOP MOTION DUE TO DRY FRICTION 

I SET T TO INITIAL TIME+HDT 

CRI_I_ DERIV TO CRI—CULRTE STRTE DERIVATIVES 

[DO FOR ERCH COMPONENT XN STRTE 






CRU_ STOPFR 

CALL DERIV 

[DO FOR ERCH COMPONENT XN STRTE 



■ 



CALL STOPFR 

SET T TO INITIAL TIME -*• TIME STEP 

CALL DERXV 

□O FOR ERCH COMPONENT IN STATE 


SET DELf TO LAST DELZ+STATE DERXV- *HOT ] 



SET STRTE VECTOR TO SRVE STRTE VECTOR-^OELZ/3. 


CRI_I_ STOPFR 
COLL DERIV 
RETURN 
END 
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2.2.23 ENDSIM 


Subroutine ENDSIM doses any files opened during execution of the re- 
sponse simulation portion of ROBSIM. 


SUBROUTINE ENDSIM 


tV 


FILE 

3 

IS 

OPEN 


v/F 


CLOSE 

LU3 


□ 

(NULL) 

T\ 


FILE 

6 

IS 

OPEN 


y/F 


CLOSE 

LU6 



CNULL) 

tV 


FILE 

13 

IS 

OPEN 


/f 


CLOSE 

LU13 



CNULL) 

t\. 


FILE 

14 

IS 

OPEN 




CLOSE 

LU14 



CNULL) 

T\ 


FILE 

16 

IS 

OPEN 


WKt 


CLOSE 

LU16 



CNULL) 

TV 


FILE 

16 

IS 

OPEN 




CLOSE 

LUIS 



CNULL) 

RETURN 

1 _ 





END 


* 
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2.2.24 SEGTASK 

Subroutine SEGTASK is called from TASK and allows the user to create a 
file that specifies manipulator motion using the following task 
commands: 

1) Pick up object; 

2) Place object at specified location; 

3) Move arm; 

4) Hold current position; 

5) Change end effector reference point; 

6) Operator control (not implemented yet); 

7) Set control mode for response simulation; 

8) Sensor of end effector position. 

The user is prompted for initial joint angles and other necessary data 
after which a sequence of task commands may be implemented. 
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2.2.25 BCNTRLR 

Subroutine BCNTRLR is called from REQUIR to obtain the position, 
velocity and acceleration for each moving base at each processing time 
step. If the variable IBDATA was set to 1 earlier, the base data are 
obtained by reading an existing file that contains just these data. If 
IBDATA equals 2, the base values are calculated from the base motion 
profiles. Subroutines RATEPRO and BPCNTRL are called for the base po- 
sition control calculations and BRCNTRL is called for the base rate 
control calculation. 



Rev A* October 1985 


UJ I ,! 
Z h ^ 
H CE S 

Lot. 

2 (D H 

0 H 

cr n 


moN) 


IU z 

, z s 

h (E 1_ F 
(y z n 5 
LL llJ H h 

I- C o B 
(f) ig: 0 
ui kr , 


u (EJ . h h 

0 Hi m H H (fl 

lli Q I H U .* 


§ H * j h 

i» gh 

ffi <r 8 

“ ID 2 

IT h 

o n i- 
il y « 

Jr Hi S 
| £T * 

0 J . 


UJ 
I 
H 
h 
Q. 
0 
h f- 


^ J3h; 

• n (E H (O jT 

liJ a H u d 0 

I z h 8 m - 

u 2 H « 

r P z n j 

r~ n u fc H 


0 kl 
IU 


s n i 

u i-ff 

H HO 


□ 

j 

II 

DC 

Ql 

h 

IjJ 

Z 

h 

0 

(E 

1 

OC 

ID 

J 

J 

J 

J 

(I 

CE 

u 

U 


J 

J fE 

Jt 

crz 

uU 


DC 


ID 


X Q 1 

uj 5 



B-1244 


DO UNTIL 
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2.3.1 SEGINIT 

Subroutine SEGINIT is called from CNTRLR at the beginning of each new 
motion profile segment. If the segment is to define motion of the 
manipulator, the sensor and target data or the coefficients of the 
polynomials defining the motion rates, the desired positions and orien- 
tations are read from the motion history file. If it is a nonmotion 
segment, the appropriate subroutines are called or variables are de- 
fined to ensure these actions are carried out. If position control is 
specified, the motion deltas for the current time segment are cal- 
culated. 



SUBROUTINE SEGINIT 
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Subroutine GRASP is called from SEGINIT when the option flag IOP equals 
1 (this denotes that the arm is to grasp a load object). The subroutine 
first checks to make sure the arm is not already holding an object and 
that the desired object is not being held by another arm. The location 
and orientation of the load object are then defined with respect to the 
end-effector coordinate system. This ensures that the object's loca- 
tion and orientation with respect to the world coordinate system will 
be updated correctly during a move and that the display shows the object 
moving with the arm. The end— effector mass properties are modified to 
include the load object to ensure the correct system response. 


SUBROUTINE GRASP 


DESIGNATED RRM NOT CURRENTLY 
HOLDING AN OBJECT 


\ DESIRED OBJECT NOT HELD BY 

T\ ANOTHER RRM 

"7? 


CRLL MRTMPY TO GET OBJECT LOCRL 



CG VECTOR IN WORLD COOR SYSTEM 

- 

cn 

t » i 

CRLCULRTE TOOL ORIGIN TO OBJECT 

cn 

1—1 J 

CO 

CG VECTOR IN WORLD COOR 

CO 

CD 

CRLL MRTMPY TO GET RBOVE 

Q 

1 

VECTOR IN TOOL COOR SYSTEM 

•“ 

LlJ 

CD 


Li-1 

CL 

CRLL MRTMPY TO PUT TOOL- ORIGIN TO 

CD 

CO 

OBJECT ORIGIN VECTOR IN TOOL. COOR SYST 

CO 

t 

CO 

LLJ 

DETERMINE OBJECT TO TOOL 

V J 

LlJ 

i i i 

TRRNSFORMRTION MRTRIX 

i i j 

1 

1— 1 

CRLL. MRTMPY TO GET OBJECT LOCRL INERTIR 

1 — l 

QC 

MRTRIX IN TOOL COOR SYSTEM 

cn 


CRLL RDDMRS TO COMBINE OBJECT 



RND TOOL MRSS PROPERTIES 




RETURN 

END 
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2.3.3 RELEAS 

Subroutine RELEAS Is called from SEGINIT when the arm is to let go of a 
load object. The routine first makes sure the load object to be re- 
leased is being held by the current arm. If it is being held, the end- 
effector mass properties are reset to the values held before the object 
was picked up and MATMPY is called to obtain the location and orienta- 
tion of the load object with respect to the world. If the object is not 
being held, a message is displayed to the user. 
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2.3.4 ESCNTRL 


ESCNTRL allows control of system motion through use of the Evans and 
Sutherland extended switches (function keys) and loads the coefficients 
of the polynomial describing the motion, PCOEF. Options Include indi- 
vidual joint control or end-effector control. For end-effector control, 
either the base coordinates or tool coordinates may be used as the re- 
ference frame. Also, the controlled motion may be either translation 
or rotation. The manipulator motion is always rate-controlled rather 
than joint-controlled. The user may select which arm and joint is to 
move. 


SUBROUTINE ESCNTRL- 




STATUS FUNCTION KEYS (EXTENDED SWITCHES} EVENT QUEUE 


DO ■ B function keys w c ti vwt bo for : 


POLE STATUS OF INDIVIDUAL FUNCTION KEYS 


SET PARAMETER FOR JOINT OR END— EFFECTOR CONTROL 


SET PARAMETER SPECIFYING POSITION OR RATE CONTROL 


■rr PnRRHETER FOR TRANSLATZONSL OR ROTRTXONAU JT. MOTION 


END-EFFECTOR CONTROL 


\ TRANSLATIONAL y/ 

T N. JT. MOTION / F 

SET VELOCITY 
VALUE TO SET 
VALUE OF 12. 
INCHES/SEC 

CONVERT 30. 
OEG/SEC VELOCITY 
TO RAD/SCC 

SET MOTION 
PROFILE COEFF. . 
ELEMENTS 1. 2. 3. 
TO SET VELOCITY 

SET MOTION 
PROFILE COEFF. . 
ELEMENTS *4. S. 6. 
TO SET VELOCITY 


STOP TIME FOR SIMULATION — CURRENT TIME 


RETURN 


END 


CONVERT 

30. 

DEG/SEC 
VELOCITY 
TO RAD/SEC 


SET MOTION 
PROFILE 
COEFF. . S 
VECTOR 

ELEMENTS. TO 
SET VELOCITY 


STEP SIZE 






2.3.5 POSSPJT 


POSSPJT is the executive routine that calls handling routines for find- 
ing the position of special joints within the manipulator. Currently 
only one type of special joint can be included. 


SUBROUTINE POSSPJT 
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2.3.6 RCNTRL 


Subroutine RCNTRL is called from CNTRLR when rate control of the joints 
or end-effector is specified. If joint rate control was chosen, the 
joint rates THD are calculated from their polynomial definitions and the 
accelerations THDD by finite difference methods. For end-effector rate 
control, the defining polynomials are evaluated for the current time. 
JACOB and SLVLIN2 are then called to transform these end-effector rates 
to individual joint rates. Accelerations (of each joint) are again cal- 
culated using finite difference methods. 


SUBROUTINE RCNTRL 


X JNTTUL - EQ. 1 


LCULATE 

(JOINT 

HUES) 

CRLC RDOT RND OMDOT CEND 

EFF. RRTES3 J 

JNTTUL - EQ. 2 

S pE 

PUT RDOT 
RND OMDOT 
XN VECTOR 
ENDVEL 

TRffMaroWM ROOT RND OMOOT 
OT WORLD COOR SYSTEM 

CALCULATE THDD 
(JOINT 

ACCELERATIONS) 

ii'iMn »■ 

CRLL JRCOB TO CRLC 
JACOBIAN 

CRLL SLVLXN2 TO SOLVE FOR 
JOINT RRTES 

F 

c 

c 

UJ CO 
J CD 1 

PUT JOINT RRTES XN THD 

RRRRV 

3 ^1 -D i 

=D Z U_ Z 

^ _l O M _ 

CRLCULRTE THDD 

RETURN 

END 
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2.3.7 RATEPRO 


RATEPRO is called from subroutine CNTRLR when position control of the 
manipulator is desired. The time allowed for the move is divided into 
six equal portions. The first portion is defined to be constant accel- 
eration. The next four are constant velocity. The last is constant 
deceleration equal in magnitude to the first portion. The distance 
traveled in the whole time is set to 1 and the appropriate distance 
traveled, velocity and acceleration for each portion are calculated. 
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2.3.8 PCNTRL 


PCNTRL is called from CNTRLR when position control of the manipulator is 
to be used. Joint position control uses the segment rate profile de- 
fined by subroutine RATEPRO to calculate the joint positions, veloci- 
ties, and accelerations. End-effector position control uses the same 
rate profile to get the end-effector rates. JTPOS is then called to get 
joint positions, and JACOB and SLVLIN2 are called to get the joint velo- 
cities. Joint accelerations are calculated by finite difference me- 
thods . 


SUBROUTINE PCNTRL 


JNTTUL - EQ. 1 

TH - 

RR#DELTH+SEGTH 

EPOS = RR*DELPOS+SEGPOS 

CALCULATE THE ANGLE OF 
ROTATION 

CflL-U MRTMPY TO FIND THE 
ORIENTATION TRANSFORMATION MATRIX 

THD - 
RD*DELTH 

\ USING FORCE/TORQUE OR / 

T\ COMPLIRNCE CONTROL /F 

SET POSREF CNULL3 

AND ORREF 

THDD = 
RDD-h-DELTH 

CRLL JTPOS TO GET JOINT 
POSITIONS 

CALCULATE ENDVEL. END 
EFFECTOR VELOCITY 

CALL SETUP 
TO CALC ALL 
POSITIONS 
IN WORLD 
COORDINATES 

CRLL JRCOB RND SLVLIN2 TO 
GET JOINT VELOCITIES 

THDD — 

CTHD— OLDTHD3 /STPPRO 

RETURN 

END 
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2.3.9 CABSM 

CABSM uses a recursive technique to compute the absolute angular and 
translational velocity and acceleration of each joint/link combination 
in the system. 
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2.3.10 FORCE 

Subroutine FORCE is called from DYNAM to calculate the force exerted on 
each joint. The force at the end-effector is determined first. PTACC 
is called to find link centroid accelerations, and the forces caused by 
these accelerations are added to the end-effector forces to find the 
force at each joint. If the system includes multiple arms on a moving 
base, the reaction force at the base is a sum of the individual base 
reaction forces from each arm that is attached to the base. 


Rev A* October 1985 
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2.3.11 TORQUE 

Subroutine TORQUE is called from DYNAM to calculate individual joint 
torques. The torques at the end-effector are determined first. 

Torques at the preceding joint are then calculated by adding the 
torques attributable to link inertias and centroid forces to the end- 
effector torques. The routine works back toward the base of the 
manipulator* adding the torques caused by inertias and centroid forces 
to the cumulative torques thus far to obtain the current joint 
torques. If the system includes multiple arms on a moving base, the 
total reaction torque at the base is the sum of all the individual 
reaction torques at the base from each arm that is attached to the base. 
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2.3.12 ACTORQ 


Subroutine ACTORQ calculates actuator drive torques for each joint. It 
is called from REQUIR when running requirements analysis and from NLINK 
when running response simulation. This routine first solves for the 
free axis of each joint and the component of joint reaction torque about 
this axis. The torque needed to overcome inertia and viscous and dry 
friction are added to the joint reaction torques to obtain a total actu- 
ator drive torque for each joint. 


SUBROUTINE ACTORQ 


INCREMENT KRRM 


INCREMENT N 

SET NRXIS TO RXIS OF ROTATION 
BRSED ON JOINT TYPE 

\ NRXIS . NE- O / 

SOLVE FOR TERM OF 
RCTURTOR TORQUE DUE 
TO LINK N OF REVOLUTE 
JOINT 

SOLVE FOR TERM OF 
RCTURTOR TORQUE DUE 
TO LINK N OF SLIDING 
JOINT 

RDD LINK N MOTOR INERTIR RND 
FRICTION TERMS TO RCTURTOR 

TORQUE 

DO UNTIL. N - NUMBER OF JOINTS IN 
KRRM 


DO UNTIL- KRRM — NRRM 


RETURN 

END 


i 
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2.3.13 REQPRT 


Subroutine REQPRT is called from OUTREQ to write run data to an output 
file if this option was requested by the user. Data written to this 
file includes time, angular position, velocity and acceleration, trans- 
lational position, velocity and acceleration, and joint force and torque 
vectors . 


SUBROUTINE REQPRT 
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2.3.14 REQSOF 

REQSOF is called from OUTREQ to write a simulation playback file if 
this option was requested by the user. The simulation playback file 
contains joint angular positions, task commands and load objects flags 
as a function of time and is used to replay the motion that occurred 
during a requirements analysis run without doing the calculations nor- 
mally associated with that run. 
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SUBROUTINE REQSOF 




2.3.15 REQTRQ 


REQTRQ Is called from OUTREQ Co write a file of actuator torques as a 
function of time If this option was chosen by the user. These data may 
then be used to run a response simulation run. 
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OF POOR QUALITY 

2.3.16 REQPLT 

Subroutine REQPLT is called from OUTREQ to write a file of various man- 
ipulator parameters as a function of time during a requirements analysis 
run. This file may then be used to create x-y plots of these parameters 
as a function of time. 


SUBROUTINE REQPLT 


OflTfl PLOT FILE MEROER INFORMATION FOR EACH Pl_OT FXUE TYPE 

SET TIME TOLERANCE FOR CHECK WHEN BEGINNING FXL.E WRITE 

RT START TIME 


(NULL) 



f>^ BRIEF PL-OT PACKAGE TYPE ^>1 


(NULL) 


imsiTK ttii» jt. mumjumm ^am. . mmol vat— . mmol wrrai ■ . MCTusrroa tophx 

[>^ END-EFFECTOR PL-OT PACKAGE TYPE 


(NULL) 

***** — ^ 1 (NULL) 

uhztc TZH& Foa. op bno-cpp. • rowcc rt eno-ot. . tqmuk mr epo-cpp. 

JOINT POSITION PLOT PACKAGE TYPE ^<| 

RT START TIME 

(NULL) 

"« ■" iBT 1 | CNULLJ 


X RERCTION FORCES PLOT PRCKRGE TYPE 

>s^ RT STRRT TIME 

(NULL) 


WRITE TIME# JT • FORCE VECTORS. TORQUE VECTORS 

>x^ COMBINATION OF ABOVE FOUR PLOT PACKAGE TYPES ^ 


RT STRRT TIME / 



(NULL) 

1 


i 

UAXTS TIM*. JT. RIM*. PQ*. . RO*. . RNO. V«U. . RWQ. RCC. . PO«C. • TOP. . 


ACT. TOR. 

ZX 

WRITE TIME. ENO-EFF. PNG. POS. . 

POS. . FORCE. TORQUE 


RETURN 

END 
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2.3.17 POSGRDJT 

POSGRDJT computes the position of the intermediate joint in a special 
joint combination called a "Gordy Joint.” This position is selected to 
satisfy a constraint on the three joints in this combination. 


SUBROUTINE POSGRDJT 



SETUP X2. ,X3. Y3. RND 

20 WITH COORDINATE 
VECTORS 

RXIS 



COMPUTE COEFFICIENTS R. 

B. C RND D USING 
VECTORS 

THESE 


s 

D LESS 

THRN ZERO 


j/r 


PRINT ■DXSCRXMXNRNT ERRORS 
UIRRNXNG 


(NULL) 


ki 


X DENOMINATOR P — ZERO 


DTI— I = PI 


ROD DTH TO DISPLACEMENT OF INTERMEDIATE JOINT 


DTH - 2 TIMES 
RTRN ( CD — 03 /R) 



COMPUTE 

NEW POSITION 


t\ 

DIRECTION OF 

RESULT IS WRONG 

/f 


SUBTRACT DTH BRCK OFF OF JOINT DISPLACEMENT 



DENOMINATOR 

R = ZERO 



DTH = 0-0 

DTH = 2 TIMES 
RTRN C C— D— BD /R3 


RDD DTH TO JOINT 

DISPLRCEMENT 



COMPUTE NEW POSITION 


(NULL) 
















I 


2.3.18 JTPOS 

JTPOS is an iterative routine for finding a set of joint angles corres- 
ponding to a desired hand position and orientation. The error DPOS in 
position is calculated and then ORERR is called to find the orientation 
error and transform it into a rotation vector. This rotation vector is 
combined with DPOS, giving DP. The Jacobian relating hand motion to 
joint motion is computed and the set of six linear equations [J](DTG) - 
(DP) is solved for the joint updates DTH. This procedure is repeated 
until the desired position is obtained. 


SUBROUTINE JTPOS 

INITIRLI2E TOLERANCES. LIMITS AND 

SCALING FRCTORS 


COMPUTE POSITION ERROR DR 


COMPUTE ORIENTATION ERROR DPHI 

\ TOTAL POSITION ERROR NOT 

T LESS THAN TOLERANCE 

^ 

4 

CALL JACOB TO EVALUATE 
JACOBIAN 



CALL SLVLIN2 TO COMPUTE 
JOINT UPDRTES DTH 



RDD DTH TO CURRENT JOINT 
POSITIONS 



EVALUATE NEW END— EFFECTOR 

POSITION 




DO UNTIL. TOTAL POSITION ERROR LESS 
THAN TOLERANCE 
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2.3.19 CVTIN 


CVTIN transforms link inertia matrices from local coordinates into their 
equivalent representation in world coordinates for use in dynamic analy- 
sis. 


SUBROUTINE CVTIN 
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2*3.20 SPRGFOR 


Subroutine SPRGFOR is called from FORCE when the manipulator end- 
effector is modeled as a compliant entity* This routine calculates the 
forces and torques at the end-effector reference point caused by its 
having compliance. 


SUBROUTINE SPRGFOR 


CRLL 

MRTMPY TO GET SPRING 
POINT IN WORLD COOR 

REF 

RDD WORLD ORIGIN TO END EFFECTOR 

ORIGIN VECTOR 

FIND 

DISTRNCE BETWEEN REF 
RCTURL LOCRTION 

RND 




ROTATIONAL STIFFNESS INCLUDED 


CALL ORERR TO CRLC DELTA 
ORIENTATION 


CPU- MRTMPY TO CRLC RESULTING 
FORCES RND TORQUES 


CNULLJ 


CRLC FORCES RND TORQUES DUE TO 
LINEAR DISPLACEMENTS 


FIND FORCES RND TORQUES RT THE END 
EFFECTOR REF POINT 


RETURN 


END 
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2.3.21 CNSTFOR 


CNSTFOR Is called from subroutine FORCE to compute the force on the end- 
effector and the torque about the end-effector reference point attribut- 
able to external constraints. These values are then added to the vari- 
ables FEND and TEND. 


SUBROUTINE CNSTFOR 


COMPUTE F. FORCE ON END EFFECTOR 
DUE TO CONSTRAINT 

COMPUTE T. TORQUE ABOUT 

END 

EFFECTOR REF POINT DUE 
CONSTRRINT 

TO 

RDD F RND T TO FEND RND 

TEND 

RETURN 



END 







2.3.22 PTACC 


i 


PTACC computes the acceleration of any point in any link of either arm. 
It uses the angular velocity and acceleration of the link to find the 
acceleration of the point relative to the acceleration of the link's 
origin and adds this to the acceleration of this link origin. 


SUBROUTINE PTRCC 


OMEGA — LINK ANGULAR VELOCITY 

VEC = VECTOR 

FROM LINK ORIGIN TO 
POINT 

ALPHA = LINK 

ANGULAR RCCELERRTION 

WCV — 

OMEGR 

CROSS VEC 

WCWCV - 

OMEGR 

CROSS WCV 

ALCV - 

RLPHR 

CROSS VEC 

RESULT - ALCV 
RCCELERRTION OF 

-+■ WCWCV •+■ 
LINK ORIGIN 
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2.3.23 POSSENS 


POSSENS is called from subroutine CONTROL when one of the feedback con- 
trol laws is being used to drive a response simulation run. This rou- 
tine obtains the discrete representation of the actual joint positions 
and also determines the actual joint velocities and accelerations. 


SUBROUTINE POSSENS 



DISCRETIZE UOINT POSITION DflTfl. 

STH 

UOINT VELOCITIES. STHD = 
CSTH— OLDSTH 1 D / CTIME— SIGLRSTJ 

UOINT RCCEL. . STHDD = 
CSTH— 2. 0*0LDSTH 1 +OLDSTH23 / 
CTIME — SIGLRST3 -*--*2 

OLDSTH2 = OLDSTH 1 

OLDSTH 1 = STH 

DO UNTIL. NJ . EQ. NUMBER OF UOINTS 
IN CURRENT RRM 

RETURN 

END 
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2.3.24 SIMPRT 


SIMPRT outputs the condensed or full data printout to file. It prints 
the position, velocity, and acceleration data for the arm at the time 
when called. If input flag IPRINT equals 2, a limited amount of infor- 
mation is printed (only TH, THD, THDD, and TDR). 


SUBROUTINE SIMPRT 


DO FOR EACH ARM OF ROBOTIC SYSTEM 


WRITE CURRENT SIM- TIME 


CONVERT THETA TERMS TO I/O UNITS 
FOR ALL JOINTS OF RRM 


WRITE JT. ANGLES. VEI - ACCEI — 


AND DRIVE TORQ. 


RID CONTROL 


WRITE j=>ID CNULL3 

VARIABLES 

FULL PRINTOUT OPTED y^ 

WRITE EFFECTIVE 
INERTIA MATRIX 

CNULL3 

WRITE JOINT /LINK 
PARAMETERS 

RETURN 


END 









2.3.25 SIMPLT 


OREGK&L m 

OF POOR QUALITY 

SIMPLT allows the user to write a plot file for output* The user is 
asked to choose from among several different plot package options. The 
chosen package determines which response simulation parameters are 
written to the plot file. 

Option 1, the BRIEF PLOT PACKAGE, writes joint angular displacements, 
joint angular velocities, joint angular accelerations and drive torques. 

Option 2, the END-EFFECTOR PLOT PACKAGE, writes end-effector transla- 
tional position, force vector at the end-effector and torque vector at 
the end-effector. 

Option 3, the JOINT POSITIONS PLOT PACKAGE, writes translational joint 
positions. 

Option 4, the REACTION FORCES PLOT PACKAGE, writes force joint vectors 
and torque joint vectors. 

Option 5, the COMBINATION PLOT PACKAGE, writes all of the above— joint 
angular displacements, translational joint positions, joint angular 
velocities, joint angular accelerations, force joint vectors, torque 
joint vectors, drive torques, end-effector translational position, force 
vector at the end-effector and torque vector at the end-effector. 

Option 6, the PID CONTROL PLOT PACKAGE, writes amplifier voltages, joint 
reference positions, joint position errors, end-effector reference posi- 
tion and end-effector position error. 

Option 7, the FORCE/TORQUE PLOT PACKAGE, writes amplifier voltages, re- 
ference position, reference force, end-effector translational position, 
force vector at the end-effector, torque vector at the end-effector, 
error in position and error in f orce/torque . 


SUBROUTINE SIMPLT 


t SET Pl_OT HERDER ORTR RECORDS FOR 

ERCH PLOT TYPE 



SIM. AT STRRT TIME 




WRITE 




CNULL] 


PROMPT FOR PS—OT FX1_H TYPE TO WRITE j 




BRIEF PACKAGE CHOSEN 






(NULL] 


WRITE 

± ■ * 1 r f -A d • : 1 1 ■Hi 





ENO— EFFECTOR PRCKRGE 

CHOSEN 

_ 


WRITE HERDER RECORD IF STRRT TIME 


(NULL) 


WRITS 

TIMS* POS. • PORC. • RNO TORO. RT SNO— SFFBCTOR 





JOINT POSITION PRCKRGE CHOSEN 



WRITE HEROER RECORD IF STRRT TIME 


(NULL] 


WRITE TIME AND JOINT POS. VECTORS 




REACTION FORCES PRCKRGE CHOSEN 



| WRITE HERDER RECORD IF STRRT TIME 


(NULL] 






COMBINATION PRCKRGE 

CHOSEN 



| WRITE HERDER RECORO IF STRRT TIME ! 


(NULL] 


WRITE 

TIME. AND PLU OF THE ABOVE PARAMETERS 




IvT 

PXD CONTROL 

CHOSEN 



| WRITE HEROER RECORD IF STRRT TIME I 



WRITS 

Lr-. W-vJ.KAtW. J*. 

<RORS 


(NULL] 


WRITE 

TIME. ENO-SFFECTQR REF. PCS. • POS. PND ROT. 

ERR. 




FORCE/ TORQUE CONTROL PACKAGE 

CHOSEN 

/r 

WRITE HEROER RECORD IF STRRT TIME 



WRITE TIME. AND JOINT VOLTS. 

(NULL] 


mm Tx*m» wsr. fob. . iv. ran. /noT. vggr^ bo-«pf. fob. /ran. 

• mio 




RETURN 

END 
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2.3.26 FORTOR 


FORTOR is called from subroutine CNTRSIG if manual force/ torque control 
is used to drive a response simulation. This routine calculates the 
joint position error vectors caused by the error in the position- 
controlled components of end-effector motion. 


SUBROUTINE FORTOR 

POSITION ERROR VECTOR. ERPOS - 
POSREF— ROS 

REMOVE FORCE CONTROLLED COMPONENTS 


FROM VECTOR ERPOS 



CRI—L. ORERR TO DETERMINE THE 


ORIENTRTION ERROR VECTOR 

REMOVE TORQUE CONTROLLED COMPONENTS 

FROM ORIENTRTION ERROR VECTOR 

COMBINE POS. RND OR. ERROR VECTORS 
INTO THE VECTOR DELP 

CRLL JHCOB TO CRLCULflTE THE 

JRCOBIRN 

CRLL SLVLIN2 TO SOLVE FOR DELTR 
POINT POSITIONS 

SET REFERENCE POINT POSITIONS 

RETURN 

END 
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(NULL) 













2.3.27 FORREF 


FORREF is called from subroutine CNTRSI6 when manual force/torque con- 
trol is used to drive a response simulation. Individual joint torque 
error vectors are calculated from the end-effector force error and tor- 
que error vectors. 


SUBROUTINE FORREF 

CRL.CUL.nTE END EFFECTOR FORCE ERROR 

COMPONENTS 


CALCULATE END EFFECTOR TORQUE 
ERROR COMPONENTS 


STORE ERROR COMPONENTS IN VECTOR 

DELFT 

CALCULATE REFERENCE FORCE/TORQUE 

VECTOR 

CALL JACOB TO DETERMINE THE 
JACOBIAN. RJACOB 

CALCULATE JOINT TORQUES* TORJNT — 
TORJNT-DELFT*RJACOB 


RETURN 


END 











2.3.28 CMP CTRL 


CMP CTRL is called from CNTRSIG when active compliance control is used 
in a response simulation run. This subroutine first calculates end 
position deltas (ref-actual), joint control torques, and joint torque 
deltas (control-sensed). The thetas are put through a derivative con- 
trol block to get joint torques. The joint torque deltas are put 
through a lead-lag filter in parallel with an integrating control block. 
The joint control torques are summed with the other processed signals 
to get a total joint torque. This is then converted to motor amplifier 


input voltages# 



SUBROUTINE CMPCTRL 


SET TVCVT. JOINT TORQUE TO VOLTS 
CONVERSION PRCTOR 

CRLL JACOB TO 

CRLCULRTE THE JACOBIAN. 
RJACOB 

determine rjtrans. THE TRANSPOSE 

JACOBIAN 

OF THE 

CRLL MATMPY 

TO FIND TOR. THE INPUT 
TORQUES 

CRLL MRTMPY 

TO FIND TBIRS. THE 
TORQUES 

BIRS 

JOINT CONTROL TORQUES. TCTRL — 

TBIRS 

TOR 

determine tsens. sensed forces and 

TORQUES 

TORQUE DELTRS 

. DELTOR =* TCTRL -t- 

TSENS 

CRLCULRTE RJTORQ. JOINT RCTURTOR 

TORQUES 

□ RIVE 

CONVERT JOINT 

TORQUES TO INPUT VOLTRGES 

RETURN 

END 
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2.3.29 PIDCON 


i Subroutine PIDCON is called from CONTROL when a control law is used to 

drive a response simulation run. The routine takes the vector of Joint 
! position errors and, simulating a PID control loop, calculates joint 

| actuator voltages. 

! 

SUBROUTINE PIDCON 

SET JOINT RCTURTOR TORQUE TO VOLTS 

CONVERSION FRCTOR 


CRLCULRTE JOINT POSITION ERROR. 
□ELTH - THREF — STH 


CALCULATE ERRINT. THE ERROR 

INTEGRAL 


CALCULATE RJTORQ. JOINT RCTURTOR 

TORQUES 


CONVERT RCTURTOR TORQUES TO 

VOLTAGES 


RETURN 










2.3.30 PIDFOR 


Subroutine PIDFOR is called from CONTROL when force/torque control is 
being used to drive a response simulation run. This routine calculates 
the joint actuator voltages caused by the force-controlled components 
of manipulator motion. 



B-154 



2.3.31 EFINRT2 


EFINRT2 computes the effective Inertia matrix (in joint coordinates) for 
a manipulator. The effective inertia matrix is an NxN matrix that gives 
the joint torques attributable to joint accelerations. The (m,n) term 
corresponds to joints m and n and depends on the mass of the arm from 
link n to the end-effector so the program evaluates composite masses, 
centroids and inertia distributions for these “composite masses.” Each 
term of the effective inertia matrix is then evaluated as a combination 
of dot products and cross-products among the joint axis directions and 
locations and the mass parameters of the composite links (see Study Re- 
sults volume). 


SUBROUTINE EFINRT2 


INITIRLIZE TOTM 


agaaBa 


DO FOR N - LRST LINK TO FIRST 


TOTM - TOTM h 

f MASS OF LINK N 

FIND NEW COMPOSITE CENTROID 

BY ROOINQ PROPERTIES OF LINK N 

FIND NEW COMPOSITE INERTIR MRTRIX 


N IS R REVOLUTE JOINT 


DO FOR ALL JOINTS M 
FROM N TO BRSE 


\ M IS R / 

\ REVOLUTE / 

T \ JOINT / F 

EFFECTIVE ^nIrtI 

INERTIR COMPONEI 

COMPONENT CM. NJ - Z 

CM. NJ - ZERO 


DO FOR ALL JOINTS M FROM N TO BRSE 


FIND TRPNSL.RTIONRL. CONTRIBUTION TO 
I EFFECTIVE XNERTIR COMPONENT CM. NJ RNO 


EFFECTIVE INERTIR TERM CN. MJ - TERM CM. N3 


FIND ROTRTXONRL 
TERM OF 
EFFECTIVE 
XNERTIR 

COMPONENT CM. NJ 



I 
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2.3.32 NLINK 

Subroutine NLINK is called from DERIV during response simulation to 
compute the base accelerations BASACC, BASOMD and joint accelerations 
THDD. It sets the joint accelerations to zero, the base accelerations 
to zero if moving base is simulated. The requirement analysis is used 
to compute effective joint torques. If moving base is simulated, 
BDRTORQ is called to input base driving torques and the base accelera- 
tion torques are calculated and DRTORQ is called to find the joint 
driving torques. EFINRT is called to compute the effective inertias. 

If moving base is simulated, SLVMBAS is called to solve for base and 
joint accelerations. If dual arm control, SLV2ARM is called to compute 
joint accelerations. Otherwise SLVTHDD is called to solve for joint 
accelerations. 
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2.3.33 SIMLMT 


SIMLMT is called by DERIV and first checks the joint displacements 
against their limits. If any limits are exceeded, the joint position 
is set to that limit and the joint rate and acceleration are limited to 
zero. Similarly, the rate limits are checked and if any are exceeded, 
the corresponding rate is set to that limit and the acceleration is 
bounded by zero. IMOD is set if any positions or rates are modified. 


SUBROUTINE SIMLMT 

IMOD = O 

DO FOR ERCH RRM 


DO FOR ERCH UOINT N 


'l^UOXNT DISPLACEMENT EXCEEDS UPPER BOUND 


SET DISPLACEMENT TO UPPER BOUND | 



ESS ZSSJOCS3S3: 

CNULU 


IMOD - 1 



^ JOINT DISPLACEMENT EXCEEDS LOWER BOUND 


SET DISPLACEMENT TO LOWER BOUND 



U2MZT VKUOCXTY 1*0 mrcwi WWTTflN TO^gWKWTH THAN Oft EQUAL. TO 

CNULU 


IMOD - 1 



UOINT RRTE EXCEEDS UPPER 

BOUND 


SET RRTE TO UPPER BOUND 



LIMIT RCCCLERRTION TO LESS THPN OR SQURL TO EERO 

CNULLJ 


IMOD - 1 



\jOINT RRTE EXCEEDS LOWER 

BOUND 

/w 

SET RRTE TO LOWER BOUND 

i 


LIMIT ACCELERATION TO GRERTER 
THAN OR EQUAL TO ZERO 

— J 


IMOD - 1 
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2.3.34 STOPFR 

Subroutine STOPFR is called from INTGRT to simulate static friction in 
the joints during a response simulation run. If the joint velocity at 
the previous time step is not equal to zero and the sign is the opposite 
of the sign of the current time step, the current velocity and position 
delta are set to zero. If moving base is simulated, assume no friction 
at the base joints. 


B-158 



SUBROUTINE 
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2.3.35 ACTIVPIH 


ACTIVPIH sets up the flags and variables activating a peg-in-hole con- 
straint if such a constraint is included. It sets up four point con- 
straints — two each (in orthogonal directions) at the top of the hole 
and at the tip of the peg. 


SUBROUTINE flCTIVPIH 


DO FOR EACH ARM 


^ IPIH NOT EQUAL ZERO /£ 

PUT TOOL. REFERENCE POINT LOCATION INTO PEGLOC 


PUT CMINUS3 HOLE-AXIS DIRECTION INTO PEGDIR 

DEL 1 EQUALS UNIT VECTOR ALONG X-AXIS CROSS 

PEGDIR 

^ MAGNITUDE OF DEI- 1 NEAR ZERO 

□El_l - UNIT VECTOR 
ALONG Y-RXIS 

NORMRLIZE DELI 

DEL2 - UNIT VECTOR 
ALONG Z-RXIS 

OEl-2 - DELI CROSS 
PEGOIR 

INITIALIZE FLRGS FOR <4 
DOUBLE-SIDED POINT CONSTRRINTS 


POINT 1 PT PEG TIP PLUS HOLE 
RADIUS ALONG DELI 

POINT 1 RT PEG TIP PLUS HOLE 
RADIUS RLONG DEL2 

POINT 1 RT HOLE ENTRRNCE PLUS 
HOLE RRDIUS RLONG DELI 

POINT 1 RT HOLE ENTRRNCE PLUS 
HOLE RRDIUS RLONG DEL2 
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2.3.36 DRTORQ 


The DRTORQ routine calculates the torque output from each joint motor 
by using a control algorithm strategy or reading them from a file. The 
calculations are based on the torque constant for each joint and the 
armature current. 


SUBROUTINE DRTORQ 
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2.3.37 EFINRT 

EFINRT computes the effective inertia matrix for a system containing 
one or more arms on fixed or moving bases. The effective inertia ma- 
trix PHI gives the base torques, forces and joint torques attributable 
to base and joints accelerations. The dimension of PHI is (ND, ND) 

HARM 

where ND = (6 * NBAS) + 21 NJ(I) 

1=1 
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CALL SOLVE TO SOLVE FOR JOINT ACCELERATION 


2.3.38 SLVTHDD 


ORIGINAL PACS 13 
°erftl§& QU&ym 


SLVTHDD solves for unknown joint acceleration^ nn$yJn& train t reaction 
forces for a given arm state and joint driving forces. All zero- 
velocity joints are assumed to have zero acceleration. If the friction 
forces needed to produce zero acceleration are greater than the static 
friction force, the acceleration is assumed finite and the equations are 
re-solved. Similarly, the constraints are assumed active and if the re- 
sulting constraint force is in the wrong direction the constraint be- 
comes inactive and the equations are re-solved. This process is re- 
peated until all conditions on the friction forces and constraints are 
satisfied. 



SUBROUTINE SLVTHDD 


DO FOR ERCH JOINT 


JOINT RATE - ZERO RNO 3TRTXC FRICTION NONZERO 


FLZV - TRUE FLZV - FPL.SE 


INITIALIZE Fl.HR - FLZV 


NO CONSTRAINTS OR ZERO-VELOCITY JOINTS EXIST 


CALL JACOB TO EVflLUATE THE JACOBIAN 


SETUP PWIT WITH COEFFICIENTS FOR NONZERO PCCELERPTION TERMS 


TTEMP - TEFF 


CONSTRAINTS EXIST 


muatmtT putt urr u i iup uotm CNULLT 


SOLVE FOR 

UNKNOWN 

ACCEUBRRTXONS RNO 

CONSTRRINT REACTION FORCES 

□ O FOR 

EACH 

CONSTRAINT 

FORCE 



MAKE CONSTRAINT A CTIV E CNULLJ 


X. ALL CONSTRAINT CONDITIONS SATISFIED 


COMPUTE FRI C TION TORQUES RT JOINTS WHERE FLEA - TRUE 


DO FOR ALL JOINTS FOR WHICH FLZV - TRUE 


X ACCELERATION IS NONZERO 



FRICTION FORCE EXCEEDS 
FRICTION COEFFICIENT 


SET FLZR - 
FRLSE 


DO UNTIL ALL FORCE RNO CONSTRRINT CONDITIONS SRTI3FIED 


SET 


FL2A - 
TRUE 

CNULL3 


























2.3.39 LININT 


Subroutine LININT is celled to set up the coefficient for performing 
linear interpolation between two vectors. 


SUBROUTINE LININT 


v T 7 

X FIRST VALUE OF / 

X INDEPENDENT / 

X VARIABLE EQUALS / 

N. SECOND VALUE v' 

CF1 = CCO EFFICIENT 
FOR FIRST 
VECTOR] EQUALS 
ZERO! 

CF2 = CCURRENT 
VALUE OF IND. 
VRR- - FIRST 
VALUE] / CSECOND 
VRI_UE - FIRST] 

CF2 EQUALS ONE 

CF1 = 1.0 - CF2 
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2.3.40 LDVOLT 


Subroutine LDVOLT is called from DRTORQ when a file of actuator voltages 
is to be read in and used to drive a response simulation run. At the 
correct time the routine reads time and voltage from an existing file. 
LININT is called to interpolate the best voltage for the current simu- 
lation time. The control voltage is then calculated from this. 


SUBROUTINE LDVOLT 



\ CORRECT TIME TO RERD NEW 
-,>v DRTR 


RERD IN TIME 


I RERD HRRDWRRE 

VOLTRGES 

DO UNTIL N - EQ. NUMBER 
OF JOINTS 

DO UNTIL- KRRM . EQ. 

NUMBER OF RRMS 

CRLL LININT TO INTERPOLATE DRTR FOR 
CURRENT SIMULATION TIME 

CRLCULRTE SIMULATION CONTROL 
VOLTRGES 

RETURN 


END 


L6' 


(NULL) 



2.3.41 CALCI 


The CALCI subroutine calculates the amplifier current values for each 
of the joints in the system given the motor parameter values and the 
state velocity. 


I 


SUBROUTINE CflLCI 


KflRM. RRM COUNTER — O 

DC 

OF 

INCREMENT KRRM 

N. JOINT COUNTER — O 


RRMRTURE I CN. KRRM) — CRMP 
GRIN-n-VOLTRGE - BRCK 
EMF*JT. VEI_. ) /RRM RES. 

UNTIL- N — NUMBER OF UOINTS 
JR KRRM 

UNTIL- KRRM — NRRM* TOTAL NUMBER 
RRMS 

RETURN 

END 
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2.3.42 SOLVE 


SOLVE is used to solve a set of N linear equations in N unknowns. It 
sets up an identity-augmenting matrix, calls GAUSS to invert the origi- 
nal matrix and then multiplies this inverse times the right-hand side 
of the equations to obtain the resulting solution. 



SUBROUTINE SOLVE 


PUT ORIGINAL MRTRIX INTO C 


FORM IDENTITY AUGMENTING MRTRIX IN 

RUG 


CALL GRUSS TO PERFORM ELIMINATION. 
PUTTING INVERSE OF C INTO RUG 


MULTIPLY RUG BY RIGHT— HRND— SIDE OF 
ORIGINAL EQURTIONS TO GET X 
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2.3.43 GAUSS 


GAUSS performs Gauss- Jordan elimination with partial pivoting on an aug- 
mented matrix system to reduce the system to row-echelon form during the 
matrix inversion process. The largest value remaining in a column is 
used as the pivot value for that column during reduction. 


SUBROUTINE GRUSS 


START WITH ORIGINAL MRTRIX IN R RND 
IDENTITY RUGMENTING MRTRIX B 


DO FOR ERCH COLUMN I 


FIND ROW U WITH LRRGEST 

VRLUE 

R CU. IJ 


^MRGNITUDE R CU. I) NOT 

EQURL 

ZERO 

/F 


DO FOR ERCH PREVIOUSLY REDUCED ROW 


REDUCE COLUMN I OF R TO ZERO 


PERFORM SRME ROW OPERRTIONS ON B 


DO FOR ERCH ROW NOT YET REDUCED 


REDUCE COLUMN I OF R TO ZERO 


PERFORM SRME ROW OPERRTIONS ON B 


REDUCE PIVOT ROW OF R TO 
STRRT WITH 1 


PERFORM SRME OPERRTION ON B 


MOVE PIVOT ROW UP TO ROW I 


(NULL) 
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2.3.44 BASINIT 

Subroutine BASINIT is called from BCNTRLR at the beginning of each new 
base motion people segment. The coefficients of the polynomials defin- 
ing the base rates or the desired positions and orientations are read 
from the base motion profile. 
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2.3.45 BRCNTRL 

Subroutine BRCNTRL is called from BCNTRLR when rate control of the base 
is specified. The rate polynomials are used to calculate the base 
translational and angular velocities. The base positions, orientations 
and accelerations are calculated from the velocities. 
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2.3.46 BPGNTRL 

Subroutine BPCNTRL is called from BCNTRLR when position control of the 
base is to be used. The rate profile is used to calculate the base po- 
sitions, velocities and accelerations. 
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2.3.47 TRACKING 

Subroutine TRACKING is called from CNTRLR when sensor control of end- 
effector motion is chosen by the user. This simulates tracking of a 
target by a video device mounted on a manipulator end-effector. 
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2.3.48 INITTAR 

Subroutine INITTAR is called from TRACKING to obtain the coordinates of 
the target corner points. 
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2.3.49 ANGLES 

Subroutine ANGLES is called from TRACKING to calculate the Euler angles 
given a direction cosine matrix. 
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RNGLES 
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2.3.50 NEWFRAME 

Subroutine NEWFRAME is called from TRACKING to obtain the coordinates 
of a three-dimensional vector in a new coordinate system. 


B-167m 



SUBROUTINE NEWFRRME 
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2.3.51 PERSPECT 

Subroutine PERSPECT is called from TRACKING to calculate the perspec- 
tive projection of a three-dimensional vector. The result is a two- 
dimensional vector. 
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SUBROUTINE RERSPECT 
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2.3.52 HARALICKR 

Subroutine HARALICKER is called from TRACKING and calculates the point- 
ing angles of a camera relative to a rectangular target, based on the 
perspective projection of the four comers of the rectangle. 
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2.3.53 BDRTORQ 

Subroutine BDRTORQ reads base torques and forces from a file and com- 
putes new torques and forces by linear interpolation. 
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2.3.54 SLVMBAS 

Subroutine SLVMBAS solves for base and joint accelerations for a given 
arm state and driving forces. 
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SUBROUTINE SLVMBRS 
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2.4.1 SLVLIN2 


SLVLIN2 finds an optimal solution X to a linear set of equations AX ■ B 
where the magnitude of each component of X is bounded — XLIM(N)^ * X(N) 

<■ XLIM(N). The program first sets up matrices D, which forms an ortho- 
gonal basis for the reachable space of A, and C, which provides the con- 
version from D space to AH space. The matrix AN is also sec up; it con- 
tains vectors in the null space of H along with the initial solution VH 
and is used as the tableau for linear programing. Once an initial so- 
lution is found, linear programming by a modification of the Simplex me- 
thod is performed; the magnitude of the result is maximized subject to 
the constraints on X. This solution is then scaled to give the final 
solution. 


SUBROUTINE Sl_Vl_IN2 
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2.4.2 REPCOL 


REPCOL replaces column ICOL of matrix A where A represents the tableau 
for a linear programming problem, X represents the variables, and the 
limits on each variable are plus or minus one. REPCOL first finds the 
largest allowable change in the free variable (the variable that corres 
ponds to the column being replaced), and the new constraint variable 
that limits this change. The solution is updated and tableau A is modi 
fied to reflect this change of constraint variables. 


SUBROUTINE REPCOL. 


FIND WHETHER TO INCREASE OR DECRERSE 
VARIABLE CORRESPONDING TO ICOL. 

FIND ROW CIROW) WHICH ALLOWS 
SMALLEST CHRNGE IN THRT VARIABLE 

UPDATE X VECTOR TO REFLECT THRT 
CHRNGE OF VRLUE 

DO FOR ERCH ROW N OF R 


R CN. ICOL) — 

R CN- ICOL) /R CIROW. ICOL) 

DO FOR ERCH COLUMN I EXCEPT ICOL 


DO FOR ERCH ROW N 


R CN. I) - R CN. I) - 
R CN. ICOL) CIROW. I) 
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2.4.4 OUTUN 


i 


OUTUN is a function that converts a value from internal (metric) units 
to the user-specified input/ output units by dividing by a conversion 
factor. 


FUNCTION OUTUN 


CONVERT R VRI_UE FROM INTERNRL TO 


I/O UNITS USING OUTUN =VRL/CONUNIT 


RETURN 


END 



2.4.5 ICVTATD 

ICVTATD is a function that returns a digitized number when given a real 
value. If the value is outside an allowable minimum or maximum, it is 
set equal to the appropriate limit. The digitized number is then com- 
puted using the following relation: 

B = (VAL - VALMIN) * (2MBITS - 2<NBITS-1)\ 

(VALMAX - VALMIN) \ ' 

where 

NBITS * number of bits available for digitized value, 

VALMIN * minimum allowable value for VAL, 

VALMAX * maximum allowable value for VAL, 

VAL ■ output value. 

ICVTATD, the digitized number, is set equal to the closest integer to B. 


FUNCTION ICVTRTD 


VRL CRNRLOG VRLUED - L_T. 
VRLMIN (MINIMUM RL L QNED) 



VRL 


VRLMIN 


CNULL3 


VR|_ . GT. VRLMRX (MAXIMUM 
RLLOWED) 


VRL 


VRLMRX 


CNULL) 


R = (VRL— VRLMIN) / CVRLMRX-VRLMIN) 
B = fl*2**NBITS-2** (NBITS 1 ) 

XCVTRTD = CLOSEST INTEGER TO B 


RETURN 


END 
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2.4.6 BORERR 

Subroutine BORERR is used to find the change in orientation between two 
coordinate systems. The error in orientation is computed and then 
transformed into a rotation (magnitude less than pi) about a rotation 
axis referenced to the base coordinate system. - 
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The Postprocessing Function 


The program POSTDRVR is the postprocessing function driver. The fol- 
lowing set of routine functional descriptions and VCLRs (visual control logic 
representations) are the modules found in the postprocessor function of ROBSIM. 


POSTDRVR 


SETLU 


1.1.2 I 

LBR— HELP 


SIMOTION 


3.1.2 1 

HDWMOTIN 


ROBPLT 


RDSYS 
- ERRMSG 


SETUP2 

| — MATMPY 
1 3.19 I 


MATMPY I 
1.4.7 1 



GRAFIX 

2.3.2 

GRASP 

1.3.14 

ADDMAS 
1^3.15 1 

ADDMAS2 
1.3.16 | 

RCICR 


2.3.3 \ 

2.1.1 1 

1.2.11 

RELEAS 

RDSYS 

SETUP 

- MATMPY 2.2.11 

- ERRMSG 

f 3.2.2 


ESPAUS 

j — E& & Routines 

ESPAUS 2 . 2.11 
E&S Routines 


E&S Graphics 
Routines 


- E&S Routines f 4 B &S R | 0Utines “ E8t ^°^ tines 


LOGO 

— E&S Routines 


HARD-COP 


1 . 3 . 67 = 

DIALS 


2 . 2.111 

| ESPAUS 

3,2.2 — E&S Ro utines 

POSTGRAF 

I 1 r 13.20 1 


3.2.3 | 

3.2.4 | 

3.2.5 \ 

3.2.6 | 

— n 

MINMAX 

AXES 

j | SCAL 

j TICMRK 

D 13000 
Plot Routines 


— E&S Routines 


DATOUT 


E&S Graphics 
Routines 


- E&S Routines - E&S Routines 

1*3*21 121 


■LgJl— I — 

HARD-COP 


E&S Routines 
1.4.6 1 

LOGO 

- E&S Routines 


SETUP2 


MATMPY 


-$ETM 


MATMPY 


— E&S Routines 


Figure B 9. - Functional block diagram for POSTDR VR. 


FQLDOUT FiUME 


HMDOU f t 
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TAble B-VII. - PROGRAMS EMPLOYED IN POSTDRVR 


3.0 

POSTDRVR 

3.1.1 

SIMOTION 

3.1.2 

HDWMOTIN 

3.1.3 

ROBPLT 

3.2.1 

LDTHET 

3.2.2 

POSTGRAF 

3.2.3 

MINMAX 

3.2.4 

AXES 

3.2.5 

SCAL 

3.2.6 

TICKRK 
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3.0 POSTDRVR 

The program POSTDRVR is the postprocessing function driver. It operates 
in an interactive mode, prompting the user for the postprocessing option 
desired: replay robotic system simulation motion, replay simulation 

versus hardware motion, parameter versus parameter plots, or terminate 
POSTDRVR execution. For simulation replay, option 1, subroutine 
SIM0TI0N, is called. Option 2 provides a comparison of hardware and the 
corresponding simulation motion through subroutine HDWMOTIN . If option 
3 is selected, ROBPLT plots any of the data computed and written to one 
of the seven types of plot file packages during the requirements or 
simulation analysis tools functions. 


PROGRAM POSTDRVR 


SET 

PROCESSOR MOOE - 

3m 

FOR POST 

PROCESSING 


SET ERROR 

CODE - 

O 

CAUL. 

SETUU TO SET PROGRAM 

OEFRULT LOGICAL 

UNIT NUMBERS 


PROMPT USER FOR OPERATION MODE. XMODE 


RERD USER RESPONSE 

\ 



IMODE - 

SI 1 


/w 

an 

LlJ 

CO 

CO 

DO CRSE ON OPERATION MOOE 



CO 

LlJ 






an 

LL- 

C 3 

C_> 

CC 


□OCRSE 


/ 


an 

LlJ 

an 

cn 

IMODE 

IMODE 

IMODE 

/ 


1 

cc 

an 

CO 
1 — 1 

- 1 

- 2 

- 3 

/ 


CL 

CF^UL. 

5IMOTION. 

CRLL 

HOWMOTXN. 

CALL ROSSCT. 

WRITE 


az 

CJ 

LlJ 

SIM. MOTION 
REPLAY 
ROUTINE 

HARDWARE 
VS. SIM. 
MOTION 
ROUTINE 

PPFWETHW vs. 

W-OTTINa MOOUL£ 

ERROR 

MESSAGE 



OO UNTIL OPERATION MODE DESIRED IS TO TERMINATE POSTDRVR 
EXECUTION 


STOP 


END 
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3.1.1 S MOTION 

S MOTION is called during the postprocessing function to provide a re- 
play of the robotic system motion produced during a previous run of the 
requirements or simulation phase of the analysis tools function. It 
opens the chosen robotic system geometry file and simulation output file 
for each graphics replay, and calls GRAFIX with the displacements at 
each time step to update the system motion display. 


SIMOTION 
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3.1.2 HDWMOTIN 

HDWMOTIN is called during the postprocessing function to provide a re- 
play of the robotic system motion produced during the requirements/ simu- 
lation analysis tools functions versus the actual motion that occurred 
during the corresponding hardware run. It opens the chosen system geo- 
metry file, simulation output file for graphics replay and hardware file 
containing recorded joint theta values. It calls POSTGRAF with the 
hardware and simulation displacements at each time step to update the 
system motion display. 
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3.1.3 ROBPLT 


i 


ORIGINAL 

0£ POOR 


Quality 




The ROBPLT subroutine plots the contents of one of several choices for 
plot package formats on a Hewlett-Packard X-Y plotter or a VAX VT125 
graphics terminal. It uses exclusively the DI3000 plot package. 

ROBPLT requests the user to select the appropriate one of seven plot 
file types that was written at the user's discretion during the require- 
ments or simulation analysis tools functions: the brief package, the 

end-effector package, the joint positions package, the reaction forces 
package, a combination of the above four packages, the PID control pack- 
age or the force/torque control package. 


SUBROUTINE ROBPLT 







ORsGSMAL hi > v;. 

3.2.1 LDTHET OF POOR QUALITY 

The LDTHET routine loads the theta values for each joint from direct 
read of the hardware control theta values file. It is called from 
HDWMOTIN during the postprocessing function for each simulation time 
step. There is a limit of one theta signal value for each joint that 
can be read. 


SUBROUTINE LDTHET 


TV F 

REP 

RD 

THETR FLRG. IRDTHET • . ME- 1 

2 


ST 

TXMe 

SE. NEXT TIME RT WHICH TO RCRO THei'A FXUE 


M 

PCBO TIME FROM HRROWRRE THETR FILE INTO TNOCT 


a 

KflRM — O 



■1 

INCREMENT KRRM 



I 

•men. o_*sst thct mb - thctn «nbxt twtmi ra * ~rr. ) 




m -rwm T ** TW *"=” 


t GO 
1 

DO 

UNTIL KRRM — NRRM 


CPU. lxnxnt to compute new tmstr control, sxgnpl. time 



KflRM — O 




INCREMENT KRRM 




z 

a 

0 


LLJ 



INCREMENT N 


_) 



THtN.KFRM». THETR - BSETWBBN ™ETU 




DO 

UNTIL N - NUMBER OF UOINTS IN KRRM 



□c 

UNTIL KRRM * NRRM 



RETURN 

— ' END 1 
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3.2.2 POSTGRAF 

Subroutine POSTGRAF provides the motion graphics capability in the post- 
processing function for HDWMOTIN, a replay of the simulation motion ver- 
sus actual hardware motion. The value of the difference in the simula- 
tion and hardware thetas is displayed, along with the environment, ro- 
botic arms, targets and load objects. 
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3.2.3 MINMAX 


MINMAX searches the postprocessor plot file for the maximum and minimum 
values to be used In scaling the axes of the plot. The x and y mlnlmums 
and ma-jf-Kmuns are found for all parameters the user chooses for plotting. 


SUBROUTINE MINMRX 


RgflO HEBQgR INFORMATIO N CONTAINING XPLC CTYFC PUOT1 • 







gs I NPUT — START PARAMETER ON RECORD 
§§ I INCREMENT NPLT 

NPUT - PSRBMCTEB CHOSEN TO PI_OT 
s o-^ 1 — » — Tfcjy — ~ I (NUI_1_3 


IDO UNTIL NJNT - NUMBER OF RECORDS FOR KRPM 



i 


i 
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3.2.4 AXES 


Subroutine AXES draws the x and y axes for a plot during the x-y 
plotting option of the postprocessor. 


SUBROUTINE AXES 


min BNP MAX ZERO CROSSING CHECK F1_RGS — o 


XMXN VALUE 


MIN 2EBO CROSSXNS FURS — 


CNUL.t-3 




XMAX VRl_UE — 0 


X MAX ZERO CROSSINS FURS — 1 


CNUL-L-J 


23 


YMXN VALUE — O 


Y MIN EERO CROSSINS FUAS - 1 


CNUL.L.3 


YMAX VRt-UE — O 


MPX ZERO CROSSINS FURS - 1 


CNUL.1—J 


13 


Y MIN ZERO CROSSING FLAG - 1 


SET V 
ORIGIN 
- O. 


CHECK — Y MHX VRL-Ug ✓ Y MUM VWLJJE 


CHECK IS NEGATIVE 


SET Y ORIGIN - O. 




Y ORISIN 


Y MIN VALUE 


CRL.L. JMOVE TO MOVE PEN TO CXMIN. YORG3 COOROINRTE 


IchLL JORBU TO DRHH X— AXIS FROM ORISIN TO CXMRX. YORS3 




X MIN ZERO CROSSING FL-RG 


CD 


CO 


CHECK — X MPX VPI_UE X X MIN VALUE 


CHECK IS NEGATIVE 


SET X ORIGIN — O. 


X ORIGIN - X MIN 
VALUE 


CRt-L. JMOVE TO MOVE PEN TO tXORG. YMIN3 COOROINRTE 


| CRl_L- JOR RLI TO ORRWI Y-AXIS PROM ORISIN TO (XOBS. VMPX3 

RETURN 
“ END 






3.2.5 SCAL 

For both user-selected automatic or specified scaling of the postpro- 
cessor plot file, routine SCAL is called from the ROBPLT option. It - 
chooses the most appropriate scale for the x- and y-axis tic marks. It 
finds the exponent of the scale base, the tic mark spacing and the min- 
imum tic mark value. The minimum value, XI, to be used for the scale, 
and DX, the scale increment between tic marks, are chosen to satisfy 
specific constraints. 


SUBROUTINE SCAL 


RDD 

R SMALL TOLERANCE TO THE MINIMUM 
VRLUE. FOR TRUNCRTION 


SUBTRACT R SMALL TOLERANCE FROM THE 
MAXIMUM VALUE. FOR TRUNCRTION 

FIRST TRIAL VALUE SCALE 
CMRX — MIN3 /NUMBER 

INCREMENT » DX. — 
PARTITIONS 



SET EXPONENT — 

LOG OF DX 


\^EXPO IS NEGRTIVE RND 

NOT RN 

INTEGER 


EXPO - EXPO - 1. 

CNUL-L.) 

SET 

BRSE = lO. ^INTEGER PRRT 

OF EXPO 


k_ 

RUTOMRTIC SCALING IS 

SET 


CHOOSE OX THE MRXZMUH OF !• 2» 

5 OR lO TIMES R POWER OP lO. 



CNUl_L_3 

SET 

MINIMUM TIC MARK VALUE - INTEGRAL 
MULTIPLE OF STEP SIZE 


RETURN 

END 
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3.2.6 TICMRK 


Routine TICMRK actually draws and labels the tic marks for a plot during 
the postprocessor function. 


SUBROUTINE TICMRK 

COMPUTE X RND V RXIS LENGTHS 

DRAW TIC MARKS OVER RANGE OF X— 0X13 ORIGIN TO 

XMRX VR1— UE 


DO RT X-RXIS TEC MARK FREQUENCY 


— 
j DETERMINE 

LEN6TH OF THE CURRENT TXC MRRK LABEL 

■ 

LABEL THE X— RXIS 

TIC MRRKS WITH VRI-UES 

■ 

DETERMINE 

LENGTH OF 

THE EXPONENT FOR THE X-RXIS 
SCRL.E 

OUTPUT 

EXPONENT 

FOR THE X— RXIS SCALE 

DRRW TXC 

MRRKS OVER RRNGE OF Y-RXXS ORXGXN TO 
YMAX VALUE 

DO RT Y 

-RXIS TIC MRRK FREQUENCY 

DETERMINE 

LENGTH OF THE CURRENT TXC MRRK LRBEL. 

■ 

URBEL. THE Y-RXIS 

TIC MRRKS WITH VALUES 

■ 

DETERMINE 

LENGTH OF 

thc ^sssr FOR THC v ~ axx3 

OUTPUT 

EXPONENT 

FOR THE Y-RXIS SCALE 

RETURN 

END 
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THE PREPROCESSING FUNCTION 
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The program PREPDRVR is the preprocessor function driver. It operates in an 
interactive mode, prompting the user to for the preprocessor option desired. 
Valid options are, create or modify a CAD/CAM object file or terminate 
PREPDRVR program execution. Figure B-10 shows the functional diagram for 
PREPDRVR and Table B-VIII lists the subroutine employed. 


l 

i 


i 



Figure B-10. Functional Block Diagram for PREPDRVR 
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TABLE B-V I I I. -PROGRAMS EMPLOYED IN PREPDRVR 


4.0 

PREPDRVR 

4.1.1 

BLDCAD 

4.2.1 

TRANSF 

4.2.2 

LINE 

4.2.3 

POINT 

4.2.4 

CURVE 

4.2.5 

SPLINE 

4.2.6 

GRAFCAD 
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4.0 PREPDRVR 

The program PREPDRVR is the Preprocessor ftmction driver. It operates 
in an interactive mode, prompting the user for the preprocessor option 
desired. Valid options are, currently: create or modify a CAD/CAM ob- 

ject file or terminate PREPDRVR program execution. 
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4.1.1 BLDCAD 


BLDCAD is called in the preprocessor driver. It reads a CAD/CAM 
initial graphics exchange specification (I6ES) - formatted file, calls 
the appropriate entity routine to fill the real number and integer data 
arrays (for graphics routine interaction), and if graphics display is 
opted by the user, calls a graphics routine to display the entities or 
an Evans and Sutherland device. The file of IGES data may be saved for 
input during the system definition function detailed graphics 
generation for arms, loads, environment or targets. The format read 
from the IGES database follows the documentation for version 2.0 
published by the National Bureau of Standards. 
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4.2.1 TRANSF 

TRANSF CAD/CAM I6ES read entity-associated transformation data and 
loads the transformation array for the rotations and translations to be 
applied to the entity before graphics display. 



SUBROUTINE TRRNSF 
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END 
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4.2.2 LINE 

LINE reads CAD/CAM I6ES line endpoint entity data and loads the line 
array for the graphics display. 
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4.2.3 POINT 

POINT reads CAD/CAM I6ES point entity data and loads the points array 
for the graphics display. 
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4.2.4 CURVE 

CURVE reads CAD/CAM I6ES circular arc data and loads the arc points ar- 
ray for the graphics display. 
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4.2.5 SPLINE 

(not implemented yet) 
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displays the CAD/CAM I6ES entity data on an Evans and 
and graphics device. 


B-194 



